Changeset 51 in flair-src for trunk/lib/FlairSensorActuator/src/NmeaGps.cpp
- Timestamp:
- 07/26/16 17:32:57 (8 years ago)
- File:
-
- 1 moved
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairSensorActuator/src/NmeaGps.cpp
r42 r51 4 4 // %flair:license} 5 5 // created: 2013/08/23 6 // filename: Gps.cpp6 // filename: NmeaGps.cpp 7 7 // 8 8 // author: Guillaume Sanahuja … … 11 11 // version: $Id: $ 12 12 // 13 // purpose: objet integrant le recepteur gps mb80013 // purpose: Base class for GPS using NMEA sentances 14 14 // 15 15 // 16 16 /*********************************************************************/ 17 17 18 #include " Gps.h"18 #include "NmeaGps.h" 19 19 #include "geodesie.h" 20 20 #include <Euler.h> 21 #include <cvmatrix.h>22 21 #include <DataPlot1D.h> 23 22 #include <Tab.h> … … 31 30 #include <Vector3D.h> 32 31 #include <Label.h> 32 #include <GpsData.h> 33 33 #include <string.h> 34 34 … … 41 41 namespace sensor { 42 42 43 Gps::Gps(const FrameworkManager *parent, string name, NMEAFlags_t NMEAFlags)43 NmeaGps::NmeaGps(const FrameworkManager *parent, string name, NMEAFlags_t NMEAFlags) 44 44 : IODevice(parent, name) { 45 45 this->NMEAFlags = NMEAFlags; … … 50 50 51 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 } 52 Err("Enable at least the GGA sentence\n"); 53 } 54 55 /* 79 56 if ((NMEAFlags & GST) != 0) { 80 57 desc->SetElementName(index, 0, "dev_lat"); … … 83 60 index += 3; 84 61 } 85 output = new cvmatrix((IODevice *)this, desc, floatType); 86 AddDataToLog(output); 62 */ 87 63 88 64 // station sol … … 97 73 position = new GeoCoordinate((IODevice *)this, "position", 0, 0, 0); 98 74 99 fix = FixQuality_t::Invalid;100 nb_sat = 0;101 75 take_ref = false; 102 nb_sat_label->SetText("nb_sat: %i", nb_sat); 103 fix_label->SetText("fix: %i", fix); 104 } 105 106 Gps::~Gps() { 76 77 gpsData = new GpsData(this); 78 AddDataToLog(gpsData); 79 80 nb_sat_label->SetText("nb_sat: %i", gpsData->GetNumberOfSatellites()); 81 fix_label->SetText("fix: %i", gpsData->GetFixQuality()); 82 } 83 84 NmeaGps::~NmeaGps() { 107 85 nmea_parser_destroy(&parser); 108 86 delete main_tab; 109 87 } 110 88 111 void Gps::UseDefaultPlot(void) { 112 int index = 0; 89 const GpsData *NmeaGps::GetDatas(void) const { 90 return gpsData; 91 } 92 93 void NmeaGps::GetDatas(core::GpsData **outGpsData) const { 94 *outGpsData = gpsData; 95 } 96 97 void NmeaGps::UseDefaultPlot(void) { 113 98 plot_tab = new Tab(tab, "Mesures"); 114 99 115 100 if ((NMEAFlags & GGA) != 0) { 116 101 e_plot = new DataPlot1D(plot_tab->NewRow(), "e", -10, 10); 117 e_plot->AddCurve( output->Element(index));102 e_plot->AddCurve(gpsData->Element(GpsData::East)); 118 103 n_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "n", -10, 10); 119 n_plot->AddCurve( output->Element(index + 1));104 n_plot->AddCurve(gpsData->Element(GpsData::North)); 120 105 u_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "u", -10, 10); 121 u_plot->AddCurve(output->Element(index + 2)); 122 index += 3; 106 u_plot->AddCurve(gpsData->Element(GpsData::Up)); 123 107 } 124 108 if ((NMEAFlags & VTG) != 0) { 125 109 ve_plot = new DataPlot1D(plot_tab->NewRow(), "ve", -10, 10); 126 ve_plot->AddCurve( output->Element(index));110 ve_plot->AddCurve(gpsData->Element(GpsData::EastVelocity)); 127 111 vn_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "vn", -10, 10); 128 vn_plot->AddCurve(output->Element(index + 1)); 129 index += 2; 112 vn_plot->AddCurve(gpsData->Element(GpsData::NorthVelocity)); 130 113 } 131 114 … … 135 118 } 136 119 137 DataPlot1D * Gps::EPlot(void) const {120 DataPlot1D *NmeaGps::EPlot(void) const { 138 121 if ((NMEAFlags & GGA) != 0) { 139 122 return e_plot; … … 144 127 } 145 128 146 DataPlot1D * Gps::NPlot(void) const {129 DataPlot1D *NmeaGps::NPlot(void) const { 147 130 if ((NMEAFlags & GGA) != 0) { 148 131 return n_plot; … … 153 136 } 154 137 155 DataPlot1D * Gps::UPlot(void) const {138 DataPlot1D *NmeaGps::UPlot(void) const { 156 139 if ((NMEAFlags & GGA) != 0) { 157 140 return u_plot; … … 162 145 } 163 146 164 DataPlot1D * Gps::VEPlot(void) const {147 DataPlot1D *NmeaGps::VEPlot(void) const { 165 148 if ((NMEAFlags & VTG) != 0) { 166 149 return ve_plot; … … 171 154 } 172 155 173 DataPlot1D * Gps::VNPlot(void) const {156 DataPlot1D *NmeaGps::VNPlot(void) const { 174 157 if ((NMEAFlags & VTG) != 0) { 175 158 return vn_plot; … … 180 163 } 181 164 182 Layout *Gps::GetLayout(void) const { return sensor_tab; } 183 184 Tab *Gps::GetPlotTab(void) const { return plot_tab; } 185 186 TabWidget *Gps::GetTab(void) const { return tab; } 187 188 uint16_t Gps::NbSat(void) const { 189 output->GetMutex(); 190 uint16_t result = nb_sat; 191 output->ReleaseMutex(); 192 return result; 193 } 194 195 Gps::FixQuality_t Gps::FixQuality(void) const { 196 output->GetMutex(); 197 FixQuality_t result = fix; 198 output->ReleaseMutex(); 199 return result; 200 } 201 202 void Gps::SetRef(void) { take_ref = true; } 203 204 void 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 212 void Gps::parseFrame(const char *frame, int frame_size) { 165 Layout *NmeaGps::GetLayout(void) const { return sensor_tab; } 166 167 Tab *NmeaGps::GetPlotTab(void) const { return plot_tab; } 168 169 TabWidget *NmeaGps::GetTab(void) const { return tab; } 170 171 void NmeaGps::SetRef(void) { take_ref = true; } 172 173 void NmeaGps::GetEnu(Vector3D *point) { 174 gpsData->GetMutex(); 175 gpsData->GetEnu(point->x,point->y,point->z); 176 gpsData->ReleaseMutex(); 177 } 178 179 void NmeaGps::parseFrame(const char *frame, int frame_size) { 213 180 214 181 int result; … … 222 189 223 190 if (result == 1) { 191 nmea_info2pos(&info, &pos); 224 192 // Printf("%s\n",frame); 225 // Printf("nb:%i fix:%i lat:%f long:%f alt:%f vel:%f226 // angle:%f\n",pack.satinuse,pack.sig,info.lat,info.lon,info.elv,info.speed*1000./3600.,info.direction);227 // Printf("lat:%f long:%f\n",pos.lat,pos.lon); 228 output->GetMutex(); // on utilise le mutex de l'outputpour fix et nb_sat229 if ( (int)fix !=pack.sig) {230 fix = (FixQuality_t)pack.sig;231 fix_label->SetText("fix: %i", fix);232 } 233 if ( nb_sat!= pack.satinuse) {234 nb_sat = pack.satinuse;235 nb_sat_label->SetText("nb_sat: %i", nb_sat);236 } 237 output->ReleaseMutex();238 239 nmea_info2pos(&info, &pos);193 // 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); 194 //Printf("lat:%f long:%f\n",pos.lat,pos.lon); 195 196 gpsData->GetMutex(); // on utilise le mutex de gpsData pour fix et nb_sat 197 if (gpsData->GetFixQuality() != (GpsData::FixQuality_t)pack.sig) { 198 gpsData->SetFixQuality((GpsData::FixQuality_t)pack.sig); 199 fix_label->SetText("fix: %i", pack.sig); 200 } 201 if (gpsData->GetNumberOfSatellites() != pack.satinuse) { 202 gpsData->SetNumberOfSatellites(pack.satinuse) ; 203 nb_sat_label->SetText("nb_sat: %i", pack.satinuse); 204 } 205 gpsData->ReleaseMutex(); 206 207 gpsData->SetLla(Euler::ToDegree(pos.lat), Euler::ToDegree(pos.lon),info.elv); 240 208 position->SetCoordinates(Euler::ToDegree(pos.lat), Euler::ToDegree(pos.lon), 241 209 info.elv); … … 257 225 // Printf("lon:%f lat:%f elv:%f\n",pos.lon,pos.lat,info.elv); 258 226 259 // on prend une fois pour toute le mutex et on fait des accès directs 260 output->GetMutex(); 261 output->SetValueNoMutex(0, 0, e); 262 output->SetValueNoMutex(1, 0, n); 263 output->SetValueNoMutex(2, 0, u); 264 265 int index = 3; 227 gpsData->SetEnu(e,n,u); 228 266 229 if ((NMEAFlags & VTG) != 0) { 267 output->SetValueNoMutex(index, 0, 268 info.speed * 1000. / 3600. * 269 sin(Euler::ToRadian(info.direction))); 270 output->SetValueNoMutex(index + 1, 0, 271 info.speed * 1000. / 3600. * 272 cos(Euler::ToRadian(info.direction))); 273 index += 2; 274 } 230 gpsData->SetVelocity(info.speed * 1000. / 3600. * sin(Euler::ToRadian(info.direction)), 231 info.speed * 1000. / 3600. * cos(Euler::ToRadian(info.direction))); 232 }/* 275 233 if ((NMEAFlags & GST) != 0) { 276 234 // Thread::Printf("dev_lon:%f dev_lat:%f … … 280 238 output->SetValueNoMutex(index + 2, 0, info.dev_elv); 281 239 index += 3; 282 } 283 output->ReleaseMutex(); 284 285 output->SetDataTime(GetTime());286 ProcessUpdate( output);240 }*/ 241 242 243 gpsData->SetDataTime(GetTime()); 244 ProcessUpdate(gpsData); 287 245 } 288 246 }
Note:
See TracChangeset
for help on using the changeset viewer.