Changeset 51 in flair-src
- Timestamp:
- Jul 26, 2016, 5:32:57 PM (8 years ago)
- Location:
- trunk
- Files:
-
- 16 edited
- 2 moved
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairCore/src/FrameworkManager_impl.cpp
r45 r51 618 618 void FrameworkManager_impl::AddDeviceToLog(IODevice *device) { 619 619 if (logger_defined == false) { 620 Err("SetupLogger() was not called, not startinglog\n");620 Warn("SetupLogger() was not called, not adding to log\n"); 621 621 return; 622 622 } -
trunk/lib/FlairCore/src/GpsData.cpp
r45 r51 18 18 #include "GpsData.h" 19 19 #include "Euler.h" 20 #include "IODataElement.h" 20 21 #include <math.h> 21 22 #include <string.h> … … 37 38 plotableData = inPlotableData; 38 39 39 size = 0; 40 switch (plotableData) { 41 case GpsData::Latitude: 42 case GpsData::Longitude: 43 size = 8; 44 break; 45 case GpsData::Altitude: 46 case GpsData::East: 47 case GpsData::North: 48 case GpsData::Up: 49 case GpsData::EastVelocity: 50 case GpsData::NorthVelocity: 51 size=4; 52 break; 53 case GpsData::NumberOfSatellites: 54 case GpsData::FixQuality: 55 size=1; 56 break; 57 } 40 58 } 41 59 … … 43 61 44 62 void CopyData(char *dst) const { 45 46 47 } 48 49 FloatType const &GetDataType(void) const { return dataType; } 63 double latitude,longitude; 64 float altitude,east,north,up,eastVelocity,northVelocity; 65 66 gpsdata->GetLla(latitude,longitude,altitude); 67 uint8_t numberOfSatellites=gpsdata->GetNumberOfSatellites(); 68 GpsData::FixQuality_t fixQuality=gpsdata->GetFixQuality(); 69 gpsdata->GetEnu(east,north,up); 70 gpsdata->GetVelocity(eastVelocity,northVelocity); 71 72 switch (plotableData) { 73 case GpsData::Latitude: 74 memcpy(dst, &latitude, sizeof(latitude)); 75 break; 76 case GpsData::Longitude: 77 memcpy(dst, &longitude, sizeof(longitude)); 78 break; 79 case GpsData::Altitude: 80 memcpy(dst, &altitude, sizeof(altitude)); 81 break; 82 case GpsData::NumberOfSatellites: 83 memcpy(dst, &numberOfSatellites, sizeof(numberOfSatellites)); 84 break; 85 case GpsData::FixQuality: 86 memcpy(dst, &fixQuality, sizeof(fixQuality)); 87 break; 88 case GpsData::East: 89 memcpy(dst, &east, sizeof(east)); 90 break; 91 case GpsData::North: 92 memcpy(dst, &north, sizeof(north)); 93 break; 94 case GpsData::Up: 95 memcpy(dst, &up, sizeof(up)); 96 break; 97 case GpsData::EastVelocity: 98 memcpy(dst, &eastVelocity, sizeof(eastVelocity)); 99 break; 100 case GpsData::NorthVelocity: 101 memcpy(dst, &northVelocity, sizeof(northVelocity)); 102 break; 103 default: 104 gpsdata->Err("data type unavailable.\n"); 105 break; 106 } 107 } 108 109 DataType const &GetDataType(void) const { 110 switch (plotableData) { 111 case GpsData::Latitude: 112 case GpsData::Longitude: 113 return doubleType; 114 break; 115 case GpsData::Altitude: 116 case GpsData::East: 117 case GpsData::North: 118 case GpsData::Up: 119 case GpsData::EastVelocity: 120 case GpsData::NorthVelocity: 121 return floatType; 122 break; 123 case GpsData::NumberOfSatellites: 124 case GpsData::FixQuality: 125 return UInt8Type; 126 break; 127 default: 128 return dummyType; 129 } 130 } 50 131 51 132 private: 52 133 const GpsData *gpsdata; 53 134 GpsData::PlotableData_t plotableData; 54 FloatType dataType;55 135 }; 56 136 … … 60 140 Warn("n>1 not supported\n"); 61 141 62 AppendLogDescription("latitude", floatType);63 AppendLogDescription("longitude", floatType);142 AppendLogDescription("latitude", doubleType); 143 AppendLogDescription("longitude", doubleType); 64 144 AppendLogDescription("altitude", floatType); 65 66 145 AppendLogDescription("nb_sat",UInt8Type); 146 AppendLogDescription("fix quality",UInt8Type); 147 AppendLogDescription("east", floatType); 148 AppendLogDescription("north", floatType); 149 AppendLogDescription("up", floatType); 150 AppendLogDescription("east velocity", floatType); 151 AppendLogDescription("north velocity", floatType); 152 153 numberOfSatellites=0; 154 fixQuality=FixQuality_t::Invalid; 67 155 } 68 156 69 157 GpsData::~GpsData() {} 70 158 159 void GpsData::GetLla(double &outLatitude, double &outLongitude, float &outAltitude) const { 160 GetMutex(); 161 outLatitude=latitude; 162 outLongitude=longitude; 163 outAltitude=altitude; 164 ReleaseMutex(); 165 } 166 167 void GpsData::SetLla(double inLatitude,double inLongitude,float inAltitude) { 168 GetMutex(); 169 latitude=inLatitude; 170 longitude=inLongitude; 171 altitude=inAltitude; 172 ReleaseMutex(); 173 } 174 175 void GpsData::GetEnu(float &outEast, float &outNorth,float &outUp) const { 176 GetMutex(); 177 outEast=east; 178 outNorth=north; 179 outUp=up; 180 ReleaseMutex(); 181 } 182 183 void GpsData::SetEnu(float inEast, float inNorth,float inUp) { 184 GetMutex(); 185 east=inEast; 186 north=inNorth; 187 up=inUp; 188 ReleaseMutex(); 189 } 190 191 192 void GpsData::GetVelocity(float &outEastVelocity, float &outNorthVelocity) const { 193 GetMutex(); 194 outEastVelocity=eastVelocity; 195 outNorthVelocity=northVelocity; 196 ReleaseMutex(); 197 } 198 199 void GpsData::SetVelocity(float inEastVelocity, float inNorthVelocity) { 200 GetMutex(); 201 eastVelocity=inEastVelocity; 202 northVelocity=inNorthVelocity; 203 ReleaseMutex(); 204 } 205 206 uint8_t GpsData::GetNumberOfSatellites(void) const { 207 return numberOfSatellites; 208 } 209 210 void GpsData::SetNumberOfSatellites(uint8_t inNumberOfSatellites) { 211 numberOfSatellites=inNumberOfSatellites; 212 } 213 214 GpsData::FixQuality_t GpsData::GetFixQuality(void) const { 215 return fixQuality; 216 } 217 218 void GpsData::SetFixQuality(FixQuality_t inFixQuality) { 219 fixQuality=inFixQuality; 220 } 71 221 72 222 IODataElement *GpsData::Element(PlotableData_t data_type) const { 73 74 //return new GpsDataElement(this, name, data_type); 223 string name; 224 switch (data_type) { 225 case GpsData::Latitude: 226 name = "Latitude"; 227 break; 228 case GpsData::Longitude: 229 name = "Longitude"; 230 break; 231 case GpsData::Altitude: 232 name = "Altitude"; 233 break; 234 case GpsData::NumberOfSatellites: 235 name = "NumberOfSatellites"; 236 break; 237 case GpsData::FixQuality: 238 name = "FixQuality"; 239 break; 240 case GpsData::East: 241 name = "East"; 242 break; 243 case GpsData::North: 244 name = "North"; 245 break; 246 case GpsData::Up: 247 name = "Up"; 248 break; 249 case GpsData::EastVelocity: 250 name = "EastVelocity"; 251 break; 252 case GpsData::NorthVelocity: 253 name = "NorthVelocity"; 254 break; 255 } 256 257 return new GpsDataElement(this, name, data_type); 75 258 } 76 259 … … 78 261 GetMutex(); 79 262 80 //Queue(&dst, &rawAcc.x, sizeof(rawAcc.x)); 263 Queue(&dst, &latitude, sizeof(latitude)); 264 Queue(&dst, &longitude, sizeof(longitude)); 265 Queue(&dst, &altitude, sizeof(altitude)); 266 Queue(&dst, &numberOfSatellites, sizeof(numberOfSatellites)); 267 Queue(&dst, &fixQuality, sizeof(FixQuality_t)); 268 Queue(&dst, &east, sizeof(east)); 269 Queue(&dst, &north, sizeof(north)); 270 Queue(&dst, &up, sizeof(up)); 271 Queue(&dst, &eastVelocity, sizeof(eastVelocity)); 272 Queue(&dst, &northVelocity, sizeof(northVelocity)); 81 273 82 274 ReleaseMutex(); -
trunk/lib/FlairCore/src/GpsData.h
r45 r51 14 14 15 15 #include <io_data.h> 16 #include <IODataElement.h>17 #include <Vector3D.h>18 16 19 17 namespace flair { 20 18 namespace core { 19 20 class IODataElement; 21 class Vector3D; 21 22 22 23 /*! \class GpsData … … 35 36 std::string GetDescription() const { return "gps data"; } 36 37 size_t GetSize() const { 37 return 0; 38 size_t size = 0; 39 size += 2*doubleType.GetSize(); // Latitude, Longitude 40 size += floatType.GetSize(); // Altitude 41 size += UInt8Type.GetSize(); // NumberOfSatellites 42 size += UInt8Type.GetSize(); // FixQuality_t 43 size += 5*floatType.GetSize();//e,n,u,ve,vn 44 return size; 38 45 } 39 46 … … 46 53 */ 47 54 typedef enum { 48 55 Latitude /*! latitude in degrees */, 56 Longitude /*! longitude in degrees */, 57 Altitude /*! altitude */, 58 NumberOfSatellites /*! number of satellites */, 59 FixQuality /*! fix quality */, 60 East /*! east */, 61 North /*! north */, 62 Up /*! up */, 63 EastVelocity /*! east velocity*/, 64 NorthVelocity /*! north velocity*/, 49 65 } PlotableData_t; 66 67 /*! 68 \enum FixQuality_t 69 \brief Fix qualty indicators 70 */ 71 enum class FixQuality_t : uint8_t { 72 Invalid = 0, /*!< invalid */ 73 Gps = 1, /*!< Gps */ 74 DGps = 2, /*!< Differential Gps */ 75 Pps = 3, /*!< Pps */ 76 Rtk = 4, /*!< RTK */ 77 RtkFloat = 5, /*!< RTK float */ 78 Estimated = 6, /*!< Estimated */ 79 Manual = 7, /*!< Manual */ 80 Simulation = 8, /*!< Simulation */ 81 }; 50 82 51 83 /*! … … 77 109 IODataElement *Element(PlotableData_t data_type) const; 78 110 111 /*! 112 * \brief Get latitude, longitude and altitude 113 * 114 * This method is mutex protected. 115 * 116 * \param latitude latitude 117 * \param longitude longitude 118 * \param altitude altitude 119 * 120 */ 121 void GetLla(double &latitude, double &longitude, 122 float &altitude) const; 123 124 /*! 125 * \brief Set latitude, longitude and altitude 126 * 127 * This method is mutex protected. 128 * 129 * \param latitude latitude 130 * \param longitude longitude 131 * \param altitude altitude 132 * 133 */ 134 void SetLla(double latitude,double longitude, 135 float altitude); 136 137 138 /*! 139 * \brief Get east, north and up 140 * 141 * This method is mutex protected. 142 * 143 * \param east east 144 * \param north north 145 * \param up up 146 * 147 */ 148 void GetEnu(float &east, float &north, 149 float &up) const; 150 151 /*! 152 * \brief Set east, north and up 153 * 154 * This method is mutex protected. 155 * 156 * \param east east 157 * \param north north 158 * \param up up 159 * 160 */ 161 void SetEnu(float east, float north, 162 float up); 163 164 /*! 165 * \brief Get east and north velocities 166 * 167 * This method is mutex protected. 168 * 169 * \param eastVelocity east velocity 170 * \param northVelocity north velocity 171 * 172 */ 173 void GetVelocity(float &eastVelocity, float &northVelocity) const; 174 175 /*! 176 * \brief Set east and north velocities 177 * 178 * This method is mutex protected. 179 * 180 * \param eastVelocity east velocity 181 * \param northVelocity north velocity 182 * 183 */ 184 void SetVelocity(float eastVelocity, float northVelocity); 185 186 /*! 187 * \brief Get number of satellites 188 * 189 * \return number of satellites 190 * 191 */ 192 uint8_t GetNumberOfSatellites(void) const; 193 194 /*! 195 * \brief Set number of satellites 196 * 197 * \param numberOfSatellites number of satellites 198 * 199 */ 200 void SetNumberOfSatellites(uint8_t numberOfSatellites); 201 202 /*! 203 * \brief Get fix quality 204 * 205 * \return fix quality 206 * 207 */ 208 FixQuality_t GetFixQuality(void) const; 209 210 /*! 211 * \brief Set fix quality 212 * 213 * \param fixQuality fix quality 214 * 215 */ 216 void SetFixQuality(FixQuality_t fixQuality); 79 217 80 218 Type const &GetDataType() const { return dataType; } … … 91 229 void CopyDatas(char *dst) const; 92 230 93 94 95 231 void Queue(char **dst, const void *src, size_t size) const; 96 232 Type dataType; 233 double latitude,longitude; 234 float altitude; 235 uint8_t numberOfSatellites; 236 FixQuality_t fixQuality; 237 float east,north,up,eastVelocity,northVelocity; 97 238 }; 98 239 -
trunk/lib/FlairCore/src/io_data.cpp
r15 r51 26 26 DummyType dummyType; 27 27 FloatType floatType; 28 DoubleType doubleType; 28 29 SignedIntegerType Int8Type(8); 29 30 SignedIntegerType Int16Type(16); 31 UnsignedIntegerType UInt8Type(8); 32 UnsignedIntegerType UInt16Type(16); 30 33 31 34 io_data::io_data(const Object *parent, string name, int n) -
trunk/lib/FlairCore/src/io_data.h
r15 r51 58 58 extern SignedIntegerType Int16Type; 59 59 60 class UnsignedIntegerType : public ScalarType { 61 public: 62 UnsignedIntegerType(size_t sizeInBits) : ScalarType(sizeInBits / 8) {} 63 std::string GetDescription() const { 64 return "uint" + std::to_string(GetSize() * 8) + "_t"; 65 }; 66 }; 67 extern UnsignedIntegerType UInt8Type; 68 extern UnsignedIntegerType UInt16Type; 69 60 70 class FloatType : public ScalarType { 61 71 public: … … 64 74 }; 65 75 extern FloatType floatType; 76 77 class DoubleType : public ScalarType { 78 public: 79 DoubleType() : ScalarType(8) {} 80 std::string GetDescription() const { return "double"; }; 81 }; 82 extern DoubleType doubleType; 66 83 67 84 /*! \class io_data -
trunk/lib/FlairFilter/src/Ahrs.h
r15 r51 18 18 19 19 namespace flair { 20 namespace core {21 class Euler;22 class Vector3D;23 class ImuData;24 class Quaternion;25 class AhrsData;26 }27 namespace gui {28 class Tab;29 class DataPlot1D;30 }31 namespace sensor {32 class Imu;33 }20 namespace core { 21 class Euler; 22 class Vector3D; 23 class ImuData; 24 class Quaternion; 25 class AhrsData; 26 } 27 namespace gui { 28 class Tab; 29 class DataPlot1D; 30 } 31 namespace sensor { 32 class Imu; 33 } 34 34 } 35 35 -
trunk/lib/FlairSensorActuator/src/Imu.h
r15 r51 17 17 18 18 namespace flair { 19 namespace core {20 class ImuData;21 class OneAxisRotation;22 }23 namespace gui {24 class Tab;25 class TabWidget;26 class GroupBox;27 class Layout;28 class DataPlot1D;29 }19 namespace core { 20 class ImuData; 21 class OneAxisRotation; 22 } 23 namespace gui { 24 class Tab; 25 class TabWidget; 26 class GroupBox; 27 class Layout; 28 class DataPlot1D; 29 } 30 30 } 31 31 -
trunk/lib/FlairSensorActuator/src/Mb800.cpp
r42 r51 28 28 29 29 Mb800::Mb800(const FrameworkManager *parent, string name, 30 SerialPort *serialport, Gps::NMEAFlags_t NMEAFlags,30 SerialPort *serialport, NmeaGps::NMEAFlags_t NMEAFlags, 31 31 uint8_t priority) 32 : Gps(parent, name, NMEAFlags), Thread(parent, name, priority) {32 : NmeaGps(parent, name, NMEAFlags), Thread(parent, name, priority) { 33 33 this->serialport = serialport; 34 34 serialport->SetRxTimeout(100000000); -
trunk/lib/FlairSensorActuator/src/Mb800.h
r15 r51 15 15 16 16 #include <Thread.h> 17 #include < Gps.h>17 #include <NmeaGps.h> 18 18 19 19 namespace flair { … … 30 30 * \brief Class for mb800 gps receiver 31 31 */ 32 class Mb800 : public core::Thread, public Gps {32 class Mb800 : public core::Thread, public NmeaGps { 33 33 public: 34 34 /*! … … 44 44 */ 45 45 Mb800(const core::FrameworkManager *parent, std::string name, 46 core::SerialPort *serialport, Gps::NMEAFlags_t NMEAFlags,46 core::SerialPort *serialport, NmeaGps::NMEAFlags_t NMEAFlags, 47 47 uint8_t priority); 48 48 -
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 } -
trunk/lib/FlairSensorActuator/src/NmeaGps.h
r42 r51 4 4 // %flair:license} 5 5 /*! 6 * \file Gps.h7 * \brief Base class for GPS 6 * \file NmeaGps.h 7 * \brief Base class for GPS using NMEA sentances 8 8 * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253 9 9 * \date 2013/08/23 … … 11 11 */ 12 12 13 #ifndef GPS_H14 #define GPS_H13 #ifndef NMEAGPS_H 14 #define NMEAGPS_H 15 15 16 16 #include <IODevice.h> … … 18 18 19 19 namespace flair { 20 namespace core {21 class cvmatrix;22 class FrameworkManager;23 class GeoCoordinate;24 class Vector3D;25 }26 namespace gui {27 class Layout;28 class DataPlot1D;29 class Tab;30 class TabWidget;31 class PushButton;32 class Map;33 class Label;34 }20 namespace core { 21 class FrameworkManager; 22 class GeoCoordinate; 23 class Vector3D; 24 class GpsData; 25 } 26 namespace gui { 27 class Layout; 28 class DataPlot1D; 29 class Tab; 30 class TabWidget; 31 class PushButton; 32 class Map; 33 class Label; 34 } 35 35 } 36 36 37 37 namespace flair { 38 38 namespace sensor { 39 /*! \class Gps 39 40 /*! \class NmeaGps 40 41 * 41 * \brief Base class for GPS 42 * \brief Base class for GPS using NMEA sentances 42 43 */ 43 class Gps : public core::IODevice {44 class NmeaGps : public core::IODevice { 44 45 public: 45 /*!46 \enum FixQuality_t47 \brief Fix qualty indicators48 */49 enum class FixQuality_t {50 Invalid = 0, /*!< invalid */51 Gps = 1, /*!< Gps */52 DGps = 2, /*!< Differential Gps */53 Pps = 3, /*!< Pps */54 Rtk = 4, /*!< RTK */55 RtkFloat = 5, /*!< RTK float */56 Estimated = 6, /*!< Estimated */57 Manual = 7, /*!< Manual */58 Simulation = 8, /*!< Simulation */59 };60 61 46 /*! 62 47 \enum NMEAFlags_t … … 72 57 * \brief Constructor 73 58 * 74 * Construct a Gps.59 * Construct a NmeaGps. 75 60 * 76 61 * \param parent parent … … 78 63 * \param NMEAFlags NMEA sentances to enable 79 64 */ 80 Gps(const core::FrameworkManager *parent, std::string name,65 NmeaGps(const core::FrameworkManager *parent, std::string name, 81 66 NMEAFlags_t NMEAFlags); 82 67 … … 85 70 * 86 71 */ 87 ~Gps(); 72 ~NmeaGps(); 73 74 /*! 75 * \brief Get GPS datas 76 * 77 * \return GpsData 78 */ 79 const core::GpsData *GetDatas(void) const; 88 80 89 81 /*! … … 148 140 */ 149 141 gui::Tab *GetPlotTab(void) const; 150 151 /*!152 * \brief Number of used satellites153 *154 * \return number of used satellites155 */156 uint16_t NbSat(void) const;157 158 /*!159 * \brief Fix Quality160 *161 * \return fix quality162 */163 FixQuality_t FixQuality(void) const;164 142 165 143 /*! … … 178 156 * \param point to store position 179 157 */ 180 void GetE NUPosition(core::Vector3D *point);158 void GetEnu(core::Vector3D *point); 181 159 182 160 protected: … … 197 175 protected: 198 176 core::GeoCoordinate *position; 177 178 /*! 179 * \brief Get GPS datas 180 * 181 * \param gpsData GPS datas 182 */ 183 void GetDatas(core::GpsData **gpsData) const; 199 184 200 185 private: … … 219 204 gui::Map *map; 220 205 gui::Label *nb_sat_label, *fix_label; 221 uint16_t nb_sat;222 FixQuality_t fix;223 206 bool take_ref; 224 207 nmeaINFO info; … … 227 210 nmeaPOS pos; 228 211 double lat_ref, long_ref, alt_ref; 229 230 // matrix 231 core::cvmatrix *output; 212 core::GpsData* gpsData; 232 213 }; 233 214 } // end namespace sensor 234 215 } // end namespace framewor 235 #endif // GPS_H216 #endif // NMEAGPS_H -
trunk/lib/FlairSensorActuator/src/Novatel.cpp
r15 r51 28 28 29 29 Novatel::Novatel(const FrameworkManager *parent, string name, 30 SerialPort *serialport, Gps::NMEAFlags_t NMEAFlags,30 SerialPort *serialport, NmeaGps::NMEAFlags_t NMEAFlags, 31 31 uint8_t priority) 32 : Gps(parent, name, NMEAFlags), Thread(parent, name, priority) {32 : NmeaGps(parent, name, NMEAFlags), Thread(parent, name, priority) { 33 33 this->serialport = serialport; 34 34 } -
trunk/lib/FlairSensorActuator/src/Novatel.h
r15 r51 15 15 16 16 #include <Thread.h> 17 #include < Gps.h>17 #include <NmeaGps.h> 18 18 19 19 namespace flair { … … 30 30 * \brief Class for Novatel gps receiver 31 31 */ 32 class Novatel : public core::Thread, public Gps {32 class Novatel : public core::Thread, public NmeaGps { 33 33 public: 34 34 /*! … … 44 44 */ 45 45 Novatel(const core::FrameworkManager *parent, std::string name, 46 core::SerialPort *serialport, Gps::NMEAFlags_t NMEAFlags,46 core::SerialPort *serialport, NmeaGps::NMEAFlags_t NMEAFlags, 47 47 uint8_t priority); 48 48 -
trunk/lib/FlairSensorActuator/src/SimuGps.cpp
r15 r51 20 20 #include <string.h> 21 21 #include <GeoCoordinate.h> 22 #include <GpsData.h> 22 23 23 24 using std::string; … … 28 29 29 30 SimuGps::SimuGps(const FrameworkManager *parent, string name, 30 Gps::NMEAFlags_t NMEAFlags, uint8_t priority)31 : Thread(parent, name, priority), Gps(parent, name, NMEAFlags) {}31 NmeaGps::NMEAFlags_t NMEAFlags, uint8_t priority) 32 : Thread(parent, name, priority), NmeaGps(parent, name, NMEAFlags) {} 32 33 33 34 SimuGps::~SimuGps() { … … 37 38 38 39 void SimuGps::Run(void) { 39 char response[200] = {0};40 int size, result;41 40 // double lat=0; 41 char buf[500]; 42 nmeaGPGGA gga; 43 nmeaGPVTG vtg; 42 44 SetPeriodMS(500); 43 45 WarnUponSwitches(true); 44 46 47 gga.sig=1; 48 gga.satinuse=2; 49 gga.lat=49.402313; 50 gga.lon=2.795463; 51 52 vtg.spn=1; 53 vtg.dir=0; 54 45 55 while (!ToBeStopped()) { 46 56 WaitPeriod(); 57 58 nmea_gen_GPGGA(buf,sizeof(buf),&gga); 59 parseFrame(buf,sizeof(buf)); 47 60 position->SetCoordinates(49.402313, 2.795463, 0); 48 61 // lat+=.5; -
trunk/lib/FlairSensorActuator/src/SimuGps.h
r15 r51 15 15 16 16 #include <Thread.h> 17 #include < Gps.h>17 #include <NmeaGps.h> 18 18 19 19 namespace flair { … … 29 29 * \brief Class for a simulation GPS 30 30 */ 31 class SimuGps : public core::Thread, public Gps {31 class SimuGps : public core::Thread, public NmeaGps { 32 32 public: 33 33 /*! … … 42 42 */ 43 43 SimuGps(const core::FrameworkManager *parent, std::string name, 44 Gps::NMEAFlags_t NMEAFlags, uint8_t priority);44 NmeaGps::NMEAFlags_t NMEAFlags, uint8_t priority); 45 45 46 46 /*! -
trunk/lib/FlairSensorActuator/src/Srf08.cpp
r15 r51 99 99 100 100 if (written < 0) { 101 Thread::Err("err eur rt_dev_write(%s)\n", strerror(-written));101 Thread::Err("error write i2c (%s)\n", strerror(-written)); 102 102 } else if (written != 2) { 103 Thread::Err("err eur rt_dev_write%i/2\n", written);103 Thread::Err("error write i2c %i/2\n", written); 104 104 } 105 105 } … … 120 120 if (written < 0) { 121 121 i2cport->ReleaseMutex(); 122 Thread::Err("err eur rt_dev_write(%s)\n", strerror(-written));122 Thread::Err("error write i2c (%s)\n", strerror(-written)); 123 123 nb_err++; 124 124 if (nb_err == 20) { 125 Thread::Err("err eur rt_dev_write(%s), too many errors\n",125 Thread::Err("error write i2c (%s), too many errors\n", 126 126 strerror(-written)); 127 127 return; … … 134 134 if (read < 0) { 135 135 if (read != -ETIMEDOUT) 136 Thread::Err("err eur rt_dev_read (%s) %i\n", strerror(-written), read);136 Thread::Err("error read i2c (%s) %i\n", strerror(-read), read); 137 137 nb_err++; 138 138 if (nb_err == 20) { 139 Thread::Err("err eur rt_dev_read(%s), too many errors\n",139 Thread::Err("error read i2c (%s), too many errors\n", 140 140 strerror(-written)); 141 141 return; … … 143 143 SleepMS(1); 144 144 } else if (read != 2) { 145 Thread::Err("err eur rt_dev_read%i/2\n", read);145 Thread::Err("error read i2c %i/2\n", read); 146 146 return; 147 147 } else if (read == 2) { … … 187 187 188 188 if (written < 0) { 189 Thread::Err("err eur rt_dev_write(%s)\n", strerror(-written));189 Thread::Err("error write i2c (%s)\n", strerror(-written)); 190 190 } else if (written != 2) { 191 Thread::Err("err eur rt_dev_write%i/2\n", written);191 Thread::Err("error write i2c %i/2\n", written); 192 192 } 193 193 } … … 211 211 212 212 if (written < 0) { 213 Thread::Err("err eur write(%s)\n", strerror(-written));213 Thread::Err("error write i2c (%s)\n", strerror(-written)); 214 214 } else if (written != 2) { 215 Thread::Err("err eur write%i/2\n", written);215 Thread::Err("error write i2c %i/2\n", written); 216 216 } 217 217 } -
trunk/lib/FlairSensorActuator/src/Srf08.h
r15 r51 40 40 * \brief Constructor 41 41 * 42 * Construct a S imuUs. Control part.42 * Construct a SRF08 sensor 43 43 * 44 44 * \param parent parent -
trunk/tools/FlairGCS/src/file_ui.cpp
r15 r51 178 178 out << time << "," << tr; 179 179 for (int i = 0; i < data_type.size(); i++) { 180 if (data_type.at(i) == "float)") { 180 if (data_type.at(i) == "double)") { 181 double *value = (double *)(data + offset); 182 offset += sizeof(double); 183 out << "," << *value; 184 } else if (data_type.at(i) == "float)") { 181 185 float *value = (float *)(data + offset); 182 186 offset += sizeof(float); … … 185 189 int8_t *value = (int8_t *)(data + offset); 186 190 offset += sizeof(int8_t); 187 float fl = (float)*value; 191 out << "," << *value; 192 } else if (data_type.at(i) == "uint8_t)") { 193 uint8_t *value = (uint8_t *)(data + offset); 194 offset += sizeof(uint8_t); 188 195 out << "," << *value; 189 196 } else {
Note:
See TracChangeset
for help on using the changeset viewer.