- Timestamp:
- Apr 8, 2016, 3:40:57 PM (9 years ago)
- Location:
- trunk
- Files:
-
- 397 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairCore/src/AhrsData.cpp
r2 r15 24 24 using std::string; 25 25 26 namespace flair { namespace core { 26 namespace flair { 27 namespace core { 27 28 28 29 /*! \class AhrsDataElement 29 30 */ 30 class AhrsDataElement: public IODataElement { 31 public: 32 33 AhrsDataElement(const AhrsData *inAhrsData,string name,AhrsData::PlotableData_t inPlotableData): 34 IODataElement(inAhrsData,name), 35 ahrsData(inAhrsData), 36 plotableData(inPlotableData) { 37 38 size=sizeof(float); 39 } 40 41 ~AhrsDataElement() {} 42 43 void CopyData(char* destination) const { 44 float data; 45 Vector3D angularRates; 46 Euler eulerAngles; 47 Quaternion quaternion; 48 ahrsData->GetQuaternionAndAngularRates(quaternion,angularRates); 49 quaternion.ToEuler(eulerAngles); 50 switch(plotableData) { 51 case AhrsData::Roll: 52 data=eulerAngles.roll; 53 break; 54 case AhrsData::Pitch: 55 data=eulerAngles.pitch; 56 break; 57 case AhrsData::Yaw: 58 data=eulerAngles.yaw; 59 break; 60 case AhrsData::RollDeg: 61 data=Euler::ToDegree(eulerAngles.roll); 62 break; 63 case AhrsData::PitchDeg: 64 data=Euler::ToDegree(eulerAngles.pitch); 65 break; 66 case AhrsData::YawDeg: 67 data=Euler::ToDegree(eulerAngles.yaw); 68 break; 69 case AhrsData::Q0: 70 data=quaternion.q0; 71 break; 72 case AhrsData::Q1: 73 data=quaternion.q1; 74 break; 75 case AhrsData::Q2: 76 data=quaternion.q2; 77 break; 78 case AhrsData::Q3: 79 data=quaternion.q3; 80 break; 81 case AhrsData::Wx: 82 data=angularRates.x; 83 break; 84 case AhrsData::Wy: 85 data=angularRates.y; 86 break; 87 case AhrsData::Wz: 88 data=angularRates.z; 89 break; 90 case AhrsData::WxDeg: 91 data=Euler::ToDegree(angularRates.x); 92 break; 93 case AhrsData::WyDeg: 94 data=Euler::ToDegree(angularRates.y); 95 break; 96 case AhrsData::WzDeg: 97 data=Euler::ToDegree(angularRates.z); 98 break; 99 default: 100 ahrsData->Err("data type unavailable.\n"); 101 data=0; 102 break; 103 } 104 memcpy(destination,&data,sizeof(float)); 105 } 106 107 const FloatType &GetDataType(void) const { 108 return dataType; 109 } 110 111 private: 112 const AhrsData *ahrsData; 113 AhrsData::PlotableData_t plotableData; 114 FloatType dataType; 115 116 }; 117 118 AhrsData::AhrsData(const Object *parent,std::string name,int n): io_data(parent,name,n), dataType(floatType) { 119 if(n>1) Warn("n>1 not supported\n"); 120 121 AppendLogDescription("q0",floatType); 122 AppendLogDescription("q1",floatType); 123 AppendLogDescription("q2",floatType); 124 AppendLogDescription("q3",floatType); 125 126 AppendLogDescription("wx",floatType); 127 AppendLogDescription("wy",floatType); 128 AppendLogDescription("wz",floatType); 31 class AhrsDataElement : public IODataElement { 32 public: 33 AhrsDataElement(const AhrsData *inAhrsData, string name, 34 AhrsData::PlotableData_t inPlotableData) 35 : IODataElement(inAhrsData, name), ahrsData(inAhrsData), 36 plotableData(inPlotableData) { 37 38 size = sizeof(float); 39 } 40 41 ~AhrsDataElement() {} 42 43 void CopyData(char *destination) const { 44 float data; 45 Vector3D angularRates; 46 Euler eulerAngles; 47 Quaternion quaternion; 48 ahrsData->GetQuaternionAndAngularRates(quaternion, angularRates); 49 quaternion.ToEuler(eulerAngles); 50 switch (plotableData) { 51 case AhrsData::Roll: 52 data = eulerAngles.roll; 53 break; 54 case AhrsData::Pitch: 55 data = eulerAngles.pitch; 56 break; 57 case AhrsData::Yaw: 58 data = eulerAngles.yaw; 59 break; 60 case AhrsData::RollDeg: 61 data = Euler::ToDegree(eulerAngles.roll); 62 break; 63 case AhrsData::PitchDeg: 64 data = Euler::ToDegree(eulerAngles.pitch); 65 break; 66 case AhrsData::YawDeg: 67 data = Euler::ToDegree(eulerAngles.yaw); 68 break; 69 case AhrsData::Q0: 70 data = quaternion.q0; 71 break; 72 case AhrsData::Q1: 73 data = quaternion.q1; 74 break; 75 case AhrsData::Q2: 76 data = quaternion.q2; 77 break; 78 case AhrsData::Q3: 79 data = quaternion.q3; 80 break; 81 case AhrsData::Wx: 82 data = angularRates.x; 83 break; 84 case AhrsData::Wy: 85 data = angularRates.y; 86 break; 87 case AhrsData::Wz: 88 data = angularRates.z; 89 break; 90 case AhrsData::WxDeg: 91 data = Euler::ToDegree(angularRates.x); 92 break; 93 case AhrsData::WyDeg: 94 data = Euler::ToDegree(angularRates.y); 95 break; 96 case AhrsData::WzDeg: 97 data = Euler::ToDegree(angularRates.z); 98 break; 99 default: 100 ahrsData->Err("data type unavailable.\n"); 101 data = 0; 102 break; 103 } 104 memcpy(destination, &data, sizeof(float)); 105 } 106 107 const FloatType &GetDataType(void) const { return dataType; } 108 109 private: 110 const AhrsData *ahrsData; 111 AhrsData::PlotableData_t plotableData; 112 FloatType dataType; 113 }; 114 115 AhrsData::AhrsData(const Object *parent, std::string name, int n) 116 : io_data(parent, name, n), dataType(floatType) { 117 if (n > 1) 118 Warn("n>1 not supported\n"); 119 120 AppendLogDescription("q0", floatType); 121 AppendLogDescription("q1", floatType); 122 AppendLogDescription("q2", floatType); 123 AppendLogDescription("q3", floatType); 124 125 AppendLogDescription("wx", floatType); 126 AppendLogDescription("wy", floatType); 127 AppendLogDescription("wz", floatType); 129 128 } 130 129 … … 132 131 133 132 Quaternion AhrsData::GetQuaternion(void) const { 134 135 136 out=quaternion;137 138 133 Quaternion out; 134 GetMutex(); 135 out = quaternion; 136 ReleaseMutex(); 137 return out; 139 138 } 140 139 141 140 Vector3D AhrsData::GetAngularRates(void) const { 142 Vector3D out; 143 GetMutex(); 144 out=angularRates; 145 ReleaseMutex(); 146 return out; 147 } 148 149 void AhrsData::GetQuaternionAndAngularRates(Quaternion &outQuaternion,Vector3D &outAngularRates) const { 150 GetMutex(); 151 outQuaternion=quaternion; 152 outAngularRates=angularRates; 153 ReleaseMutex(); 154 } 155 156 void AhrsData::SetQuaternionAndAngularRates(const Quaternion &inQuaternion,const Vector3D &inAngularRates) { 157 GetMutex(); 158 quaternion=inQuaternion; 159 angularRates=inAngularRates; 160 ReleaseMutex(); 141 Vector3D out; 142 GetMutex(); 143 out = angularRates; 144 ReleaseMutex(); 145 return out; 146 } 147 148 void AhrsData::GetQuaternionAndAngularRates(Quaternion &outQuaternion, 149 Vector3D &outAngularRates) const { 150 GetMutex(); 151 outQuaternion = quaternion; 152 outAngularRates = angularRates; 153 ReleaseMutex(); 154 } 155 156 void AhrsData::SetQuaternionAndAngularRates(const Quaternion &inQuaternion, 157 const Vector3D &inAngularRates) { 158 GetMutex(); 159 quaternion = inQuaternion; 160 angularRates = inAngularRates; 161 ReleaseMutex(); 161 162 } 162 163 163 164 void AhrsData::SetQuaternion(const Quaternion &inQuaternion) { 164 165 quaternion=inQuaternion;166 165 GetMutex(); 166 quaternion = inQuaternion; 167 ReleaseMutex(); 167 168 } 168 169 169 170 void AhrsData::SetAngularRates(const Vector3D &inAngularRates) { 170 171 angularRates=inAngularRates;172 173 } 174 175 IODataElement *AhrsData::Element(PlotableData_t plotableData) const {176 177 switch(plotableData) {178 179 name="Roll";180 181 182 name="Pitch";183 184 185 name="Yaw";186 187 188 name="Roll degree";189 190 191 name="Pitch degree";192 193 194 name="Yaw degree";195 196 197 name="Q0";198 199 200 name="Q1";201 202 203 name="Q2";204 205 206 name="Q3";207 208 209 name="Wx";210 211 212 name="Wy";213 214 215 name="Wz";216 217 218 name="Wx degree";219 220 221 name="Wy degree";222 223 224 name="Wz degree";225 226 227 228 229 230 return new AhrsDataElement(this,name,plotableData);171 GetMutex(); 172 angularRates = inAngularRates; 173 ReleaseMutex(); 174 } 175 176 IODataElement *AhrsData::Element(PlotableData_t plotableData) const { 177 string name; 178 switch (plotableData) { 179 case AhrsData::Roll: 180 name = "Roll"; 181 break; 182 case AhrsData::Pitch: 183 name = "Pitch"; 184 break; 185 case AhrsData::Yaw: 186 name = "Yaw"; 187 break; 188 case AhrsData::RollDeg: 189 name = "Roll degree"; 190 break; 191 case AhrsData::PitchDeg: 192 name = "Pitch degree"; 193 break; 194 case AhrsData::YawDeg: 195 name = "Yaw degree"; 196 break; 197 case AhrsData::Q0: 198 name = "Q0"; 199 break; 200 case AhrsData::Q1: 201 name = "Q1"; 202 break; 203 case AhrsData::Q2: 204 name = "Q2"; 205 break; 206 case AhrsData::Q3: 207 name = "Q3"; 208 break; 209 case AhrsData::Wx: 210 name = "Wx"; 211 break; 212 case AhrsData::Wy: 213 name = "Wy"; 214 break; 215 case AhrsData::Wz: 216 name = "Wz"; 217 break; 218 case AhrsData::WxDeg: 219 name = "Wx degree"; 220 break; 221 case AhrsData::WyDeg: 222 name = "Wy degree"; 223 break; 224 case AhrsData::WzDeg: 225 name = "Wz degree"; 226 break; 227 default: 228 Err("data type unavailable.\n"); 229 } 230 231 return new AhrsDataElement(this, name, plotableData); 231 232 } 232 233 233 234 void AhrsData::CopyDatas(char *dst) const { 234 235 Queue(&dst,&quaternion.q0,sizeof(quaternion.q0));236 Queue(&dst,&quaternion.q1,sizeof(quaternion.q1));237 Queue(&dst,&quaternion.q2,sizeof(quaternion.q2));238 Queue(&dst,&quaternion.q3,sizeof(quaternion.q3));239 240 Queue(&dst,&angularRates.x,sizeof(angularRates.x));241 Queue(&dst,&angularRates.y,sizeof(angularRates.y));242 Queue(&dst,&angularRates.z,sizeof(angularRates.z));243 244 } 245 246 void AhrsData::Queue(char **dst, const void *src,size_t size) const {247 memcpy(*dst,src,size);248 *dst+=size;235 GetMutex(); 236 Queue(&dst, &quaternion.q0, sizeof(quaternion.q0)); 237 Queue(&dst, &quaternion.q1, sizeof(quaternion.q1)); 238 Queue(&dst, &quaternion.q2, sizeof(quaternion.q2)); 239 Queue(&dst, &quaternion.q3, sizeof(quaternion.q3)); 240 241 Queue(&dst, &angularRates.x, sizeof(angularRates.x)); 242 Queue(&dst, &angularRates.y, sizeof(angularRates.y)); 243 Queue(&dst, &angularRates.z, sizeof(angularRates.z)); 244 ReleaseMutex(); 245 } 246 247 void AhrsData::Queue(char **dst, const void *src, size_t size) const { 248 memcpy(*dst, src, size); 249 *dst += size; 249 250 } 250 251 -
trunk/lib/FlairCore/src/AhrsData.h
r2 r15 18 18 #include <Quaternion.h> 19 19 20 namespace flair { namespace core { 20 namespace flair { 21 namespace core { 21 22 22 /*! \class AhrsData 23 * 24 * \brief Class defining AHRS datas 25 * 26 * AHRS datas consist of quaternion and rotational angles values. \n 27 * 28 */ 29 class AhrsData: public io_data { 30 public: 31 class Type: public DataType { 32 public: 33 Type(const ScalarType &inElementDataType): 34 elementDataType(inElementDataType) {} 35 const ScalarType &GetElementDataType() const {return elementDataType;} 36 std::string GetDescription() const {return "ahrs data";} 37 size_t GetSize() const { 38 return 7*elementDataType.GetSize(); 39 } 40 private: 41 const ScalarType &elementDataType; 42 }; 23 /*! \class AhrsData 24 * 25 * \brief Class defining AHRS datas 26 * 27 * AHRS datas consist of quaternion and rotational angles values. \n 28 * 29 */ 30 class AhrsData : public io_data { 31 public: 32 class Type : public DataType { 33 public: 34 Type(const ScalarType &inElementDataType) 35 : elementDataType(inElementDataType) {} 36 const ScalarType &GetElementDataType() const { return elementDataType; } 37 std::string GetDescription() const { return "ahrs data"; } 38 size_t GetSize() const { return 7 * elementDataType.GetSize(); } 43 39 44 /*! 45 \enum PlotableData_t 46 \brief Datas wich can be plotted in a DataPlot1D 47 */ 48 typedef enum { 49 Roll/*! roll */, Pitch/*! pitch */, Yaw/*! yaw */, 50 RollDeg/*! roll degree*/, PitchDeg/*! pitch degree */, YawDeg/*! yaw degree */, 51 Q0/*! quaternion 0 */, Q1/*! quaternion 1 */, Q2/*! quaternion 2 */, Q3/*! quaternion 3 */, 52 Wx/*! x filtered angular rate */, Wy/*! y filtered angular rate */, Wz/*! z filtered angular rate */, 53 WxDeg/*! x filtered angular rate degree*/, WyDeg/*! y filtered angular rate degree*/, WzDeg/*! z filtered angular rate degree*/, 54 } PlotableData_t; 40 private: 41 const ScalarType &elementDataType; 42 }; 55 43 56 /*! 57 * \brief Constructor 58 * 59 * Construct an io_data representing AHRS datas. \n 60 * 61 * \param parent parent 62 * \param name name 63 * \param n number of samples 64 */ 65 AhrsData(const Object* parent,std::string name="",int n=1); 44 /*! 45 \enum PlotableData_t 46 \brief Datas wich can be plotted in a DataPlot1D 47 */ 48 typedef enum { 49 Roll /*! roll */, 50 Pitch /*! pitch */, 51 Yaw /*! yaw */, 52 RollDeg /*! roll degree*/, 53 PitchDeg /*! pitch degree */, 54 YawDeg /*! yaw degree */, 55 Q0 /*! quaternion 0 */, 56 Q1 /*! quaternion 1 */, 57 Q2 /*! quaternion 2 */, 58 Q3 /*! quaternion 3 */, 59 Wx /*! x filtered angular rate */, 60 Wy /*! y filtered angular rate */, 61 Wz /*! z filtered angular rate */, 62 WxDeg /*! x filtered angular rate degree*/, 63 WyDeg /*! y filtered angular rate degree*/, 64 WzDeg /*! z filtered angular rate degree*/, 65 } PlotableData_t; 66 66 67 /*! 68 * \brief Destructor 69 * 70 */ 71 ~AhrsData(); 67 /*! 68 * \brief Constructor 69 * 70 * Construct an io_data representing AHRS datas. \n 71 * 72 * \param parent parent 73 * \param name name 74 * \param n number of samples 75 */ 76 AhrsData(const Object *parent, std::string name = "", int n = 1); 72 77 73 /*! 74 * \brief Element 75 * 76 * Get a pointer to a specific element. This pointer can be used for plotting. 77 * 78 * \param plotableData data type 79 * 80 * \return pointer to the element 81 */ 82 IODataElement *Element(PlotableData_t plotableData) const; 78 /*! 79 * \brief Destructor 80 * 81 */ 82 ~AhrsData(); 83 83 84 /*! 85 * \brief Set quaternion 86 * 87 * This method is mutex protected. 88 * 89 * \param quaternion quaternion 90 * 91 */ 92 void SetQuaternion(const Quaternion &quaternion); 84 /*! 85 * \brief Element 86 * 87 * Get a pointer to a specific element. This pointer can be used for plotting. 88 * 89 * \param plotableData data type 90 * 91 * \return pointer to the element 92 */ 93 IODataElement *Element(PlotableData_t plotableData) const; 93 94 94 95 * \brief Get quaternion96 97 98 99 * \return quaternion100 101 102 Quaternion GetQuaternion(void) const;95 /*! 96 * \brief Set quaternion 97 * 98 * This method is mutex protected. 99 * 100 * \param quaternion quaternion 101 * 102 */ 103 void SetQuaternion(const Quaternion &quaternion); 103 104 104 105 * \brief Set angular rates106 107 108 109 * \param angularRates angular rates110 111 112 void SetAngularRates(const Vector3D &angularRates);105 /*! 106 * \brief Get quaternion 107 * 108 * This method is mutex protected. 109 * 110 * \return quaternion 111 * 112 */ 113 Quaternion GetQuaternion(void) const; 113 114 114 115 * \brief Get angular rates116 117 118 119 * \returnangular rates120 121 122 Vector3D GetAngularRates(void) const;115 /*! 116 * \brief Set angular rates 117 * 118 * This method is mutex protected. 119 * 120 * \param angularRates angular rates 121 * 122 */ 123 void SetAngularRates(const Vector3D &angularRates); 123 124 124 /*! 125 * \brief Get both quaternion and angular rates 126 * 127 * This method is mutex protected. 128 * 129 * \param quaternion quaternion 130 * \param angularRates angular rates 131 * 132 */ 133 void GetQuaternionAndAngularRates(Quaternion &quaternion,Vector3D &angularRates) const; 125 /*! 126 * \brief Get angular rates 127 * 128 * This method is mutex protected. 129 * 130 * \return angular rates 131 * 132 */ 133 Vector3D GetAngularRates(void) const; 134 134 135 /*! 136 * \brief Set both quaternion and angular rates 137 * 138 * This method is mutex protected. 139 * 140 * \param quaternion quaternion 141 * \param angularRates angular rates 142 * 143 */ 144 void SetQuaternionAndAngularRates(const Quaternion &quaternion,const Vector3D &angularRates); 135 /*! 136 * \brief Get both quaternion and angular rates 137 * 138 * This method is mutex protected. 139 * 140 * \param quaternion quaternion 141 * \param angularRates angular rates 142 * 143 */ 144 void GetQuaternionAndAngularRates(Quaternion &quaternion, 145 Vector3D &angularRates) const; 145 146 146 const Type &GetDataType() const {return dataType;}147 private:148 /*!149 * \brief Copy datas150 151 * Reimplemented from io_data. \n152 * See io_data::CopyDatas.153 154 * \param dst destination buffer155 */156 void CopyDatas(char *dst) const;147 /*! 148 * \brief Set both quaternion and angular rates 149 * 150 * This method is mutex protected. 151 * 152 * \param quaternion quaternion 153 * \param angularRates angular rates 154 * 155 */ 156 void SetQuaternionAndAngularRates(const Quaternion &quaternion, 157 const Vector3D &angularRates); 157 158 158 void Queue(char **dst,const void *src,size_t size) const;159 const Type &GetDataType() const { return dataType; } 159 160 160 /*! 161 * \brief %Quaternion 162 * 163 */ 164 Quaternion quaternion; 161 private: 162 /*! 163 * \brief Copy datas 164 * 165 * Reimplemented from io_data. \n 166 * See io_data::CopyDatas. 167 * 168 * \param dst destination buffer 169 */ 170 void CopyDatas(char *dst) const; 165 171 166 /*! 167 * \brief Angular rates 168 * 169 */ 170 Vector3D angularRates; 172 void Queue(char **dst, const void *src, size_t size) const; 171 173 172 Type dataType; 173 }; 174 /*! 175 * \brief %Quaternion 176 * 177 */ 178 Quaternion quaternion; 179 180 /*! 181 * \brief Angular rates 182 * 183 */ 184 Vector3D angularRates; 185 186 Type dataType; 187 }; 174 188 175 189 } // end namespace core -
trunk/lib/FlairCore/src/Box.cpp
r2 r15 21 21 using std::string; 22 22 23 namespace flair 24 { 25 namespace gui 26 { 23 namespace flair { 24 namespace gui { 27 25 28 Box::Box(const LayoutPosition * position,string name,string type): Widget(position->getLayout(),name,type)29 {30 value_changed=true;31 SetVolatileXmlProp("row",position->Row());32 SetVolatileXmlProp("col",position->Col());33 26 Box::Box(const LayoutPosition *position, string name, string type) 27 : Widget(position->getLayout(), name, type) { 28 value_changed = true; 29 SetVolatileXmlProp("row", position->Row()); 30 SetVolatileXmlProp("col", position->Col()); 31 delete position; 34 32 } 35 33 36 Box::~Box() 37 { 38 core::Object::ObjectName(); 34 Box::~Box() { core::Object::ObjectName(); } 35 36 bool Box::ValueChanged(void) { 37 bool ret; 38 39 GetMutex(); 40 if (value_changed == true) { 41 value_changed = false; 42 ret = true; 43 } else { 44 ret = false; 45 } 46 ReleaseMutex(); 47 48 return ret; 39 49 } 40 50 41 bool Box::ValueChanged(void) 42 { 43 bool ret; 51 void Box::SetValueChanged(void) { value_changed = true; } 44 52 45 GetMutex(); 46 if(value_changed==true) 47 { 48 value_changed=false; 49 ret=true; 50 } 51 else 52 { 53 ret=false; 54 } 55 ReleaseMutex(); 53 void Box::GetMutex(void) const { ((Layout *)Parent())->mutex->GetMutex(); } 56 54 57 return ret; 58 } 59 60 void Box::SetValueChanged(void) 61 { 62 value_changed=true; 63 } 64 65 void Box::GetMutex(void) const 66 { 67 ((Layout*)Parent())->mutex->GetMutex(); 68 } 69 70 void Box::ReleaseMutex(void) const 71 { 72 ((Layout*)Parent())->mutex->ReleaseMutex(); 55 void Box::ReleaseMutex(void) const { 56 ((Layout *)Parent())->mutex->ReleaseMutex(); 73 57 } 74 58 -
trunk/lib/FlairCore/src/Box.h
r2 r15 16 16 #include <Widget.h> 17 17 18 namespace flair 19 { 20 namespace gui 21 { 18 namespace flair { 19 namespace gui { 22 20 23 24 21 class Layout; 22 class LayoutPosition; 25 23 26 /*! \class Box 27 * 28 * \brief Abstract class to display a box on the ground station 29 * 30 * This is an abstract class to display boxes (like CheckBox, SpinBox, etc). \n 31 * To access reimplemented box's value, use Box::GetMutex and Box::ReleaseMutex. \n 32 * Note that this mutex is in reality the one from the parent Layout. To minimize memory 33 * footprint, each Box does not have its own Mutex. 34 */ 35 class Box: public Widget 36 { 37 public: 38 /*! 39 * \brief Constructor 40 * 41 * Construct a Box. \n 42 * Type must agree with predifined (hard coded) types 43 * in ground station code. \n 44 * The Box will automatically be child of position->getLayout() Layout. After calling this method, 45 * position will be deleted as it is no longer usefull. 46 * 47 * \param position position 48 * \param name name 49 * \param type type 50 */ 51 Box(const LayoutPosition* position,std::string name,std::string type); 24 /*! \class Box 25 * 26 * \brief Abstract class to display a box on the ground station 27 * 28 * This is an abstract class to display boxes (like CheckBox, SpinBox, etc). \n 29 * To access reimplemented box's value, use Box::GetMutex and Box::ReleaseMutex. 30 *\n 31 * Note that this mutex is in reality the one from the parent Layout. To minimize 32 *memory 33 * footprint, each Box does not have its own Mutex. 34 */ 35 class Box : public Widget { 36 public: 37 /*! 38 * \brief Constructor 39 * 40 * Construct a Box. \n 41 * Type must agree with predifined (hard coded) types 42 * in ground station code. \n 43 * The Box will automatically be child of position->getLayout() Layout. After 44 *calling this method, 45 * position will be deleted as it is no longer usefull. 46 * 47 * \param position position 48 * \param name name 49 * \param type type 50 */ 51 Box(const LayoutPosition *position, std::string name, std::string type); 52 52 53 54 55 56 57 53 /*! 54 * \brief Destructor 55 * 56 */ 57 ~Box(); 58 58 59 60 61 62 63 64 65 66 67 68 69 59 /*! 60 * \brief Has the value changed since last call? 61 * 62 * This method returns the value of an internal flag 63 * which is set through SetValueChanged(). \n 64 * After calling this method, the internal flag is 65 * set to false. 66 * 67 * \return true is valued has changed since last call 68 */ 69 bool ValueChanged(void); 70 70 71 protected: 72 /*! 73 * \brief Set the value changed flag 74 * 75 * The reimplemented class must call this method when Box's value is changed. \n 76 * This method must be called with Mutex locked. Indeed, as reimplemented class 77 * also has to lock the Mutex to change the Box value, this mecanism avoid two successives 78 * lock and unlock. 79 * 80 */ 81 void SetValueChanged(void); 71 protected: 72 /*! 73 * \brief Set the value changed flag 74 * 75 * The reimplemented class must call this method when Box's value is changed. 76 *\n 77 * This method must be called with Mutex locked. Indeed, as reimplemented class 78 * also has to lock the Mutex to change the Box value, this mecanism avoid two 79 *successives 80 * lock and unlock. 81 * 82 */ 83 void SetValueChanged(void); 82 84 83 84 85 86 87 88 89 90 85 /*! 86 * \brief Get Mutex 87 * 88 * This method must be called before changing Box's value or 89 * calling SetValueChanged(). 90 * 91 */ 92 void GetMutex(void) const; 91 93 92 93 94 95 96 97 98 99 94 /*! 95 * \brief Release Mutex 96 * 97 * This method must be called after changing Box's value or 98 * calling SetValueChanged(). 99 * 100 */ 101 void ReleaseMutex(void) const; 100 102 101 102 private: 103 bool value_changed; 104 }; 103 private: 104 bool value_changed; 105 }; 105 106 106 107 } // end namespace gui -
trunk/lib/FlairCore/src/CheckBox.cpp
r2 r15 19 19 using std::string; 20 20 21 namespace flair 22 { 23 namespace gui 24 { 21 namespace flair { 22 namespace gui { 25 23 26 CheckBox::CheckBox(const LayoutPosition* position,string name,bool default_value):Box(position,name,"CheckBox") 27 { 28 //update value from xml file 29 box_value=default_value; 24 CheckBox::CheckBox(const LayoutPosition *position, string name, 25 bool default_value) 26 : Box(position, name, "CheckBox") { 27 // update value from xml file 28 box_value = default_value; 30 29 31 GetPersistentXmlProp("value",box_value);32 SetPersistentXmlProp("value",Value());33 30 GetPersistentXmlProp("value", box_value); 31 SetPersistentXmlProp("value", Value()); 32 SendXml(); 34 33 } 35 34 36 CheckBox::~CheckBox() 37 { 35 CheckBox::~CheckBox() {} 38 36 37 bool CheckBox::IsChecked(void) const { 38 bool tmp; 39 40 GetMutex(); 41 tmp = box_value; 42 ReleaseMutex(); 43 44 return tmp; 39 45 } 40 46 41 bool CheckBox::IsChecked(void) const 42 { 43 bool tmp; 44 45 GetMutex(); 46 tmp=box_value; 47 ReleaseMutex(); 48 49 return tmp; 47 int CheckBox::Value(void) const { 48 if (IsChecked() == true) 49 return 1; 50 else 51 return 0; 50 52 } 51 53 52 int CheckBox::Value(void) const 53 { 54 if(IsChecked()==true) 55 return 1; 56 else 57 return 0; 58 } 59 60 void CheckBox::XmlEvent(void) 61 { 62 GetMutex(); 63 if(GetPersistentXmlProp("value",box_value)) SetValueChanged(); 64 ReleaseMutex(); 54 void CheckBox::XmlEvent(void) { 55 GetMutex(); 56 if (GetPersistentXmlProp("value", box_value)) 57 SetValueChanged(); 58 ReleaseMutex(); 65 59 } 66 60 -
trunk/lib/FlairCore/src/CheckBox.h
r2 r15 16 16 #include <Box.h> 17 17 18 namespace flair 19 { 20 namespace gui 21 { 18 namespace flair { 19 namespace gui { 22 20 23 21 class LayoutPosition; 24 22 25 26 27 28 29 30 class CheckBox: public Box 31 { 32 public:33 /*!34 * \brief Constructor35 *36 * Construct a QCheckBox at given position.37 *38 * \param position position to display the QCheckBox39 * \param name name40 * \param default_value default value if not in the xml config file41 */42 CheckBox(const LayoutPosition* position,std::string name,bool default_value=true);23 /*! \class CheckBox 24 * 25 * \brief Class displaying a QCheckBox on the ground station 26 * 27 */ 28 class CheckBox : public Box { 29 public: 30 /*! 31 * \brief Constructor 32 * 33 * Construct a QCheckBox at given position. 34 * 35 * \param position position to display the QCheckBox 36 * \param name name 37 * \param default_value default value if not in the xml config file 38 */ 39 CheckBox(const LayoutPosition *position, std::string name, 40 bool default_value = true); 43 41 44 45 46 47 48 42 /*! 43 * \brief Destructor 44 * 45 */ 46 ~CheckBox(); 49 47 50 51 52 53 54 55 48 /*! 49 * \brief Is checked? 50 * 51 * \return true if checked 52 */ 53 bool IsChecked(void) const; 56 54 57 58 59 60 61 62 55 /*! 56 * \brief Value 57 * 58 * \return 1 if checked, 0 otherwise 59 */ 60 int Value(void) const; 63 61 64 65 66 67 68 69 70 71 62 private: 63 /*! 64 * \brief XmlEvent from ground station 65 * 66 * Reimplemented from Widget. 67 * 68 */ 69 void XmlEvent(void); 72 70 73 74 71 bool box_value; 72 }; 75 73 76 74 } // end namespace gui -
trunk/lib/FlairCore/src/ComboBox.cpp
r2 r15 19 19 using std::string; 20 20 21 namespace flair 22 { 23 namespace gui 24 { 21 namespace flair { 22 namespace gui { 25 23 26 ComboBox::ComboBox(const LayoutPosition * position,string name):Box(position,name,"ComboBox")27 {28 //update value from xml file29 box_value=0;30 GetPersistentXmlProp("value",box_value);31 SetPersistentXmlProp("value",box_value);24 ComboBox::ComboBox(const LayoutPosition *position, string name) 25 : Box(position, name, "ComboBox") { 26 // update value from xml file 27 box_value = 0; 28 GetPersistentXmlProp("value", box_value); 29 SetPersistentXmlProp("value", box_value); 32 30 33 31 SendXml(); 34 32 } 35 33 36 ComboBox::~ComboBox() 37 { 34 ComboBox::~ComboBox() {} 38 35 36 void ComboBox::AddItem(string name) { 37 SetVolatileXmlProp("item", name); 38 SendXml(); 39 39 } 40 40 41 void ComboBox::AddItem(string name) 42 { 43 SetVolatileXmlProp("item",name); 44 SendXml(); 41 int ComboBox::CurrentIndex(void) const { 42 int tmp; 43 44 GetMutex(); 45 tmp = box_value; 46 ReleaseMutex(); 47 48 return tmp; 45 49 } 46 50 47 int ComboBox::CurrentIndex(void) const 48 { 49 int tmp; 50 51 GetMutex(); 52 tmp=box_value; 53 ReleaseMutex(); 54 55 return tmp; 56 } 57 58 void ComboBox::XmlEvent(void) 59 { 60 GetMutex(); 61 if(GetPersistentXmlProp("value",box_value)) SetValueChanged(); 62 ReleaseMutex(); 51 void ComboBox::XmlEvent(void) { 52 GetMutex(); 53 if (GetPersistentXmlProp("value", box_value)) 54 SetValueChanged(); 55 ReleaseMutex(); 63 56 } 64 57 -
trunk/lib/FlairCore/src/ComboBox.h
r2 r15 16 16 #include <Box.h> 17 17 18 namespace flair 19 { 20 namespace gui 21 { 18 namespace flair { 19 namespace gui { 22 20 23 21 class LayoutPosition; 24 22 25 /*! \class ComboBox 26 * 27 * \brief Class displaying a QComboBox on the ground station 28 * 29 */ 30 class ComboBox: public Box 31 { 32 public: 33 /*! 34 * \brief Constructor 35 * 36 * Construct a QComboBox at given position. 37 * 38 * \param position position to display the QComboBox 39 * \param name name 40 */ 41 ComboBox(const LayoutPosition* position,std::string name); 23 /*! \class ComboBox 24 * 25 * \brief Class displaying a QComboBox on the ground station 26 * 27 */ 28 class ComboBox : public Box { 29 public: 30 /*! 31 * \brief Constructor 32 * 33 * Construct a QComboBox at given position. 34 * 35 * \param position position to display the QComboBox 36 * \param name name 37 */ 38 ComboBox(const LayoutPosition *position, std::string name); 42 39 43 44 45 46 47 40 /*! 41 * \brief Destructor 42 * 43 */ 44 ~ComboBox(); 48 45 49 50 51 52 53 54 55 56 46 /*! 47 * \brief Add an item 48 * 49 * Add an item to the end of the list. 50 * 51 * \param name item nam 52 */ 53 void AddItem(std::string name); 57 54 58 59 60 61 62 63 64 65 55 /*! 56 * \brief Currend index 57 * 58 * Index of the currently selected item. Items are numbered starting to 0. 59 * 60 * \return index number 61 */ 62 int CurrentIndex(void) const; 66 63 67 68 69 70 71 72 73 74 64 private: 65 /*! 66 * \brief XmlEvent from ground station 67 * 68 * Reimplemented from Widget. 69 * 70 */ 71 void XmlEvent(void); 75 72 76 77 73 int box_value; 74 }; 78 75 79 76 } // end namespace gui -
trunk/lib/FlairCore/src/ConditionVariable.cpp
r2 r15 21 21 using std::string; 22 22 23 namespace flair { namespace core { 23 namespace flair { 24 namespace core { 24 25 25 ConditionVariable::ConditionVariable(const Object* parent,string name): Mutex(parent,name) { 26 pimpl_=new ConditionVariable_impl(this); 26 ConditionVariable::ConditionVariable(const Object *parent, string name) 27 : Mutex(parent, name) { 28 pimpl_ = new ConditionVariable_impl(this); 27 29 } 28 30 29 ConditionVariable::~ConditionVariable() { 30 delete pimpl_; 31 ConditionVariable::~ConditionVariable() { delete pimpl_; } 32 33 void ConditionVariable::CondWait(void) { pimpl_->CondWait(); } 34 35 bool ConditionVariable::CondWaitUntil(Time date) { 36 return pimpl_->CondWaitUntil(date); 31 37 } 32 38 33 void ConditionVariable::CondWait(void) { 34 pimpl_->CondWait(); 35 } 36 37 bool ConditionVariable::CondWaitUntil(Time date) { 38 return pimpl_->CondWaitUntil(date); 39 } 40 41 void ConditionVariable::CondSignal(void) { 42 pimpl_->CondSignal(); 43 } 39 void ConditionVariable::CondSignal(void) { pimpl_->CondSignal(); } 44 40 45 41 } // end namespace core -
trunk/lib/FlairCore/src/ConditionVariable.h
r2 r15 18 18 class ConditionVariable_impl; 19 19 20 namespace flair 21 { 22 namespace core 23 { 20 namespace flair { 21 namespace core { 24 22 25 26 27 28 29 23 /*! \class ConditionVariable 24 * 25 * \brief Class defining a condition variable 26 * 27 */ 30 28 31 class ConditionVariable: public Mutex 32 { 33 public: 34 /*! 35 * \brief Constructor 36 * 37 * Construct a condition variable with its associated mutex. 38 * 39 * \param parent parent 40 * \param name name 41 */ 42 ConditionVariable(const Object* parent,std::string name); 29 class ConditionVariable : public Mutex { 30 public: 31 /*! 32 * \brief Constructor 33 * 34 * Construct a condition variable with its associated mutex. 35 * 36 * \param parent parent 37 * \param name name 38 */ 39 ConditionVariable(const Object *parent, std::string name); 43 40 44 45 46 47 48 41 /*! 42 * \brief Destructor 43 * 44 */ 45 ~ConditionVariable(); 49 46 50 /*! 51 * \brief Block on the condition variable 52 * 53 * This method must be called with mutex locked (see Mutex::GetMutex) by the calling thread or undefined behaviour will result. \n 54 * It atomically releases mutex and causes the calling thread to block on the condition variable. \n 55 * Only one thread can be blocked at the same time. \n 56 * Upon successful return, the mutex has been locked and is owned by the calling thread which should unlock it (see Mutex::ReleaseMutex). 57 */ 58 void CondWait(void); 47 /*! 48 * \brief Block on the condition variable 49 * 50 * This method must be called with mutex locked (see Mutex::GetMutex) by the 51 *calling thread or undefined behaviour will result. \n 52 * It atomically releases mutex and causes the calling thread to block on the 53 *condition variable. \n 54 * Only one thread can be blocked at the same time. \n 55 * Upon successful return, the mutex has been locked and is owned by the 56 *calling thread which should unlock it (see Mutex::ReleaseMutex). 57 */ 58 void CondWait(void); 59 59 60 /*! 61 * \brief Block on the condition variable with a timeout 62 * 63 * This method must be called with mutex locked (see Mutex::GetMutex) by the calling thread or undefined behaviour will result. \n 64 * It atomically releases mutex and causes the calling thread to block on the condition variable. \n 65 * Only one thread can be blocked at the same time. \n 66 * Upon successful return, the mutex has been locked and is owned by the calling thread which should unlock it (see Mutex::ReleaseMutex). 67 * 68 * \param date absolute date 69 * \return true if the condition variable is signaled before the date specified in parameter elapses, false otherwise 70 */ 71 bool CondWaitUntil(Time date); 60 /*! 61 * \brief Block on the condition variable with a timeout 62 * 63 * This method must be called with mutex locked (see Mutex::GetMutex) by the 64 *calling thread or undefined behaviour will result. \n 65 * It atomically releases mutex and causes the calling thread to block on the 66 *condition variable. \n 67 * Only one thread can be blocked at the same time. \n 68 * Upon successful return, the mutex has been locked and is owned by the 69 *calling thread which should unlock it (see Mutex::ReleaseMutex). 70 * 71 * \param date absolute date 72 * \return true if the condition variable is signaled before the date specified 73 *in parameter elapses, false otherwise 74 */ 75 bool CondWaitUntil(Time date); 72 76 73 /*! 74 * \brief Unblock threads blocked on the condition variable 75 * 76 * This method should be called with mutex locked (see Mutex::GetMutex) by the calling thread. \n 77 * In this case, upon return, the calling thread should unlock the mutex (see Mutex::ReleaseMutex). 78 * 79 */ 80 void CondSignal(void); 77 /*! 78 * \brief Unblock threads blocked on the condition variable 79 * 80 * This method should be called with mutex locked (see Mutex::GetMutex) by the 81 *calling thread. \n 82 * In this case, upon return, the calling thread should unlock the mutex (see 83 *Mutex::ReleaseMutex). 84 * 85 */ 86 void CondSignal(void); 81 87 82 83 class ConditionVariable_impl*pimpl_;84 88 private: 89 class ConditionVariable_impl *pimpl_; 90 }; 85 91 86 92 } // end namespace core -
trunk/lib/FlairCore/src/ConditionVariable_impl.cpp
r2 r15 24 24 using namespace flair::core; 25 25 26 ConditionVariable_impl::ConditionVariable_impl(ConditionVariable *self) {27 this->self=self;28 26 ConditionVariable_impl::ConditionVariable_impl(ConditionVariable *self) { 27 this->self = self; 28 int status; 29 29 30 30 #ifdef __XENO__ 31 status=rt_cond_create(&m_ResumeCond,NULL);31 status = rt_cond_create(&m_ResumeCond, NULL); 32 32 #else 33 status=pthread_cond_init(&m_ResumeCond, 0);33 status = pthread_cond_init(&m_ResumeCond, 0); 34 34 #endif 35 if(status!=0) self->Err("error creating condition variable (%s)\n",strerror(-status)); 35 if (status != 0) 36 self->Err("error creating condition variable (%s)\n", strerror(-status)); 36 37 } 37 38 38 39 ConditionVariable_impl::~ConditionVariable_impl() { 39 40 int status; 40 41 41 42 #ifdef __XENO__ 42 status=rt_cond_delete(&m_ResumeCond);43 status = rt_cond_delete(&m_ResumeCond); 43 44 #else 44 status=pthread_cond_destroy(&m_ResumeCond);45 status = pthread_cond_destroy(&m_ResumeCond); 45 46 #endif 46 if(status!=0) self->Err("error destroying condition variable (%s)\n",strerror(-status)); 47 if (status != 0) 48 self->Err("error destroying condition variable (%s)\n", strerror(-status)); 47 49 } 48 50 49 51 void ConditionVariable_impl::CondWait(void) { 50 52 int status; 51 53 #ifdef __XENO__ 52 status=rt_cond_wait(&m_ResumeCond, &self->Mutex::pimpl_->mutex,TM_INFINITE); 54 status = 55 rt_cond_wait(&m_ResumeCond, &self->Mutex::pimpl_->mutex, TM_INFINITE); 53 56 #else 54 status=pthread_cond_wait(&m_ResumeCond, &self->Mutex::pimpl_->mutex);57 status = pthread_cond_wait(&m_ResumeCond, &self->Mutex::pimpl_->mutex); 55 58 #endif 56 if(status!=0) self->Err("error (%s)\n",strerror(-status)); 59 if (status != 0) 60 self->Err("error (%s)\n", strerror(-status)); 57 61 } 58 62 59 63 bool ConditionVariable_impl::CondWaitUntil(Time date) { 60 64 int status; 61 65 #ifdef __XENO__ 62 status=rt_cond_wait_until(&m_ResumeCond, &self->Mutex::pimpl_->mutex,date);66 status = rt_cond_wait_until(&m_ResumeCond, &self->Mutex::pimpl_->mutex, date); 63 67 #else 64 struct timespec restrict; 65 restrict.tv_sec=date/1000000000; 66 restrict.tv_nsec=date%1000000000; 67 status=pthread_cond_timedwait(&m_ResumeCond, &self->Mutex::pimpl_->mutex,&restrict); 68 struct timespec restrict; 69 restrict.tv_sec = date / 1000000000; 70 restrict.tv_nsec = date % 1000000000; 71 status = pthread_cond_timedwait(&m_ResumeCond, &self->Mutex::pimpl_->mutex, 72 &restrict); 68 73 #endif 69 if (status==0) return true; 70 if(status!=ETIMEDOUT) self->Err("error (%s)\n",strerror(-status)); 71 return false; 74 if (status == 0) 75 return true; 76 if (status != ETIMEDOUT) 77 self->Err("error (%s)\n", strerror(-status)); 78 return false; 72 79 } 73 80 74 81 void ConditionVariable_impl::CondSignal(void) { 75 82 int status; 76 83 #ifdef __XENO__ 77 status=rt_cond_signal(&m_ResumeCond);84 status = rt_cond_signal(&m_ResumeCond); 78 85 #else 79 status=pthread_cond_signal(&m_ResumeCond);86 status = pthread_cond_signal(&m_ResumeCond); 80 87 #endif 81 if(status!=0) self->Err("error (%s)\n",strerror(-status)); 88 if (status != 0) 89 self->Err("error (%s)\n", strerror(-status)); 82 90 } -
trunk/lib/FlairCore/src/ConnectedSocket.cpp
r2 r15 21 21 using std::string; 22 22 23 namespace flair 24 { 25 namespace core 26 { 23 namespace flair { 24 namespace core { 27 25 28 ConnectedSocket::ConnectedSocket(const Object* parent,const std::string name):Object(parent,name) {}; 26 ConnectedSocket::ConnectedSocket(const Object *parent, const std::string name) 27 : Object(parent, name){}; 29 28 30 ConnectedSocket::~ConnectedSocket() 29 ConnectedSocket::~ConnectedSocket(){}; 31 30 32 uint16_t ConnectedSocket::ReadUInt16(Time const& timeout) { 33 char buffer[sizeof(uint16_t)]; 34 size_t alreadyReceived=0; 35 Time remainingTimeout=timeout; //ms 36 do { 37 Time beforeTime=GetTime(); //ns 38 ssize_t received=RecvMessage(buffer+alreadyReceived,sizeof(uint16_t)-alreadyReceived,remainingTimeout); 39 remainingTimeout-=(GetTime()-beforeTime)/100000; 40 if ((received<0) || (remainingTimeout<0)) throw std::runtime_error("Timeout"); 41 alreadyReceived+=received; 42 } while (alreadyReceived!=sizeof(uint16_t)); 43 uint16_t *dataInNetworkEndianness=(uint16_t*)buffer; 44 uint16_t myinteger=NetworkToHost16(*dataInNetworkEndianness); 45 return myinteger; 31 uint16_t ConnectedSocket::ReadUInt16(Time const &timeout) { 32 char buffer[sizeof(uint16_t)]; 33 size_t alreadyReceived = 0; 34 Time remainingTimeout = timeout; // ms 35 do { 36 Time beforeTime = GetTime(); // ns 37 ssize_t received = 38 RecvMessage(buffer + alreadyReceived, 39 sizeof(uint16_t) - alreadyReceived, remainingTimeout); 40 remainingTimeout -= (GetTime() - beforeTime) / 100000; 41 if ((received < 0) || (remainingTimeout < 0)) 42 throw std::runtime_error("Timeout"); 43 alreadyReceived += received; 44 } while (alreadyReceived != sizeof(uint16_t)); 45 uint16_t *dataInNetworkEndianness = (uint16_t *)buffer; 46 uint16_t myinteger = NetworkToHost16(*dataInNetworkEndianness); 47 return myinteger; 46 48 } 47 49 48 void ConnectedSocket::WriteUInt16(uint16_t const& data,Time const& timeout) { 49 uint16_t dataInNetworkEndianness=HostToNetwork16(data); 50 char *buffer=(char *)&dataInNetworkEndianness; 51 size_t alreadySent=0; 52 Time remainingTimeout=timeout; //ms 53 do { 54 Time beforeTime=GetTime(); //ns 55 ssize_t sent=SendMessage(buffer+alreadySent,sizeof(uint16_t)-alreadySent,remainingTimeout); 56 remainingTimeout-=(GetTime()-beforeTime)/100000; 57 if ((sent<0) || (remainingTimeout<0)) throw std::runtime_error("Timeout"); 58 alreadySent+=sent; 59 } while (alreadySent!=sizeof(uint16_t)); 50 void ConnectedSocket::WriteUInt16(uint16_t const &data, Time const &timeout) { 51 uint16_t dataInNetworkEndianness = HostToNetwork16(data); 52 char *buffer = (char *)&dataInNetworkEndianness; 53 size_t alreadySent = 0; 54 Time remainingTimeout = timeout; // ms 55 do { 56 Time beforeTime = GetTime(); // ns 57 ssize_t sent = SendMessage( 58 buffer + alreadySent, sizeof(uint16_t) - alreadySent, remainingTimeout); 59 remainingTimeout -= (GetTime() - beforeTime) / 100000; 60 if ((sent < 0) || (remainingTimeout < 0)) 61 throw std::runtime_error("Timeout"); 62 alreadySent += sent; 63 } while (alreadySent != sizeof(uint16_t)); 60 64 } 61 65 62 uint32_t ConnectedSocket::ReadUInt32(Time const& timeout) { 63 char buffer[sizeof(uint32_t)]; 64 size_t alreadyReceived=0; 65 Time remainingTimeout=timeout; //ms 66 do { 67 Time beforeTime=GetTime(); //ns 68 ssize_t received=RecvMessage(buffer+alreadyReceived,sizeof(uint32_t)-alreadyReceived,remainingTimeout); 69 remainingTimeout-=(GetTime()-beforeTime)/100000; 70 if ((received<0) || (remainingTimeout<0)) throw std::runtime_error("Timeout"); 71 alreadyReceived+=received; 72 } while (alreadyReceived!=sizeof(uint32_t)); 73 uint32_t *dataInNetworkEndianness=(uint32_t*)buffer; 74 uint32_t myinteger=NetworkToHost32(*dataInNetworkEndianness); 75 return myinteger; 66 uint32_t ConnectedSocket::ReadUInt32(Time const &timeout) { 67 char buffer[sizeof(uint32_t)]; 68 size_t alreadyReceived = 0; 69 Time remainingTimeout = timeout; // ms 70 do { 71 Time beforeTime = GetTime(); // ns 72 ssize_t received = 73 RecvMessage(buffer + alreadyReceived, 74 sizeof(uint32_t) - alreadyReceived, remainingTimeout); 75 remainingTimeout -= (GetTime() - beforeTime) / 100000; 76 if ((received < 0) || (remainingTimeout < 0)) 77 throw std::runtime_error("Timeout"); 78 alreadyReceived += received; 79 } while (alreadyReceived != sizeof(uint32_t)); 80 uint32_t *dataInNetworkEndianness = (uint32_t *)buffer; 81 uint32_t myinteger = NetworkToHost32(*dataInNetworkEndianness); 82 return myinteger; 76 83 } 77 84 78 void ConnectedSocket::WriteUInt32(uint32_t const& data,Time const& timeout) { 79 uint32_t dataInNetworkEndianness=HostToNetwork32(data); 80 char *buffer=(char *)&dataInNetworkEndianness; 81 size_t alreadySent=0; 82 Time remainingTimeout=timeout; //ms 83 do { 84 Time beforeTime=GetTime(); //ns 85 ssize_t sent=SendMessage(buffer+alreadySent,sizeof(uint32_t)-alreadySent,remainingTimeout); 86 remainingTimeout-=(GetTime()-beforeTime)/100000; 87 if ((sent<0) || (remainingTimeout<0)) throw std::runtime_error("Timeout"); 88 alreadySent+=sent; 89 } while (alreadySent!=sizeof(uint32_t)); 85 void ConnectedSocket::WriteUInt32(uint32_t const &data, Time const &timeout) { 86 uint32_t dataInNetworkEndianness = HostToNetwork32(data); 87 char *buffer = (char *)&dataInNetworkEndianness; 88 size_t alreadySent = 0; 89 Time remainingTimeout = timeout; // ms 90 do { 91 Time beforeTime = GetTime(); // ns 92 ssize_t sent = SendMessage( 93 buffer + alreadySent, sizeof(uint32_t) - alreadySent, remainingTimeout); 94 remainingTimeout -= (GetTime() - beforeTime) / 100000; 95 if ((sent < 0) || (remainingTimeout < 0)) 96 throw std::runtime_error("Timeout"); 97 alreadySent += sent; 98 } while (alreadySent != sizeof(uint32_t)); 90 99 } 91 100 92 101 string ConnectedSocket::ReadString(const size_t &stringSize, Time timeout) { 93 char buffer[stringSize+1]; 94 size_t alreadyReceived=0; 95 Time remainingTimeout=timeout; //ms 96 do { 97 Time beforeTime=GetTime(); //ns 98 ssize_t received=RecvMessage(buffer+alreadyReceived,stringSize-alreadyReceived,remainingTimeout); 99 remainingTimeout-=(GetTime()-beforeTime)/100000; 100 if ((received<0) || (remainingTimeout<0)) throw std::runtime_error("Timeout"); 101 alreadyReceived+=received; 102 } while (alreadyReceived!=stringSize); 103 buffer[stringSize]='\0'; 102 char buffer[stringSize + 1]; 103 size_t alreadyReceived = 0; 104 Time remainingTimeout = timeout; // ms 105 do { 106 Time beforeTime = GetTime(); // ns 107 ssize_t received = 108 RecvMessage(buffer + alreadyReceived, stringSize - alreadyReceived, 109 remainingTimeout); 110 remainingTimeout -= (GetTime() - beforeTime) / 100000; 111 if ((received < 0) || (remainingTimeout < 0)) 112 throw std::runtime_error("Timeout"); 113 alreadyReceived += received; 114 } while (alreadyReceived != stringSize); 115 buffer[stringSize] = '\0'; 104 116 105 string mystring=buffer;106 117 string mystring = buffer; 118 return mystring; 107 119 } 108 120 -
trunk/lib/FlairCore/src/ConnectedSocket.h
r2 r15 16 16 #include <Object.h> 17 17 18 namespace flair 19 { 20 namespace core 21 { 18 namespace flair { 19 namespace core { 22 20 23 /*! \class ConnectedSocket 24 * 25 * \brief Interface class encapsulating a connected socket. Preserves packets order and guaranty delivery. 26 * 27 */ 28 class ConnectedSocket: public Object { 29 public: 30 /*! 31 * \brief Constructor 32 * 33 */ 34 ConnectedSocket(const Object* parent,const std::string name); 21 /*! \class ConnectedSocket 22 * 23 * \brief Interface class encapsulating a connected socket. Preserves packets 24 *order and guaranty delivery. 25 * 26 */ 27 class ConnectedSocket : public Object { 28 public: 29 /*! 30 * \brief Constructor 31 * 32 */ 33 ConnectedSocket(const Object *parent, const std::string name); 35 34 36 37 38 39 40 35 /*! 36 * \brief Destructor 37 * 38 */ 39 ~ConnectedSocket(); 41 40 42 /*! 43 * \brief Returns a socket which listens on a specific port/address 44 * 45 * \param const Object* parentObject 46 * \param const string name 47 * \param unsigned int port 48 * \param const localAddress (defaults to any) 49 */ 50 virtual void Listen(const unsigned int port,const std::string localAddress="ANY")=0; 41 /*! 42 * \brief Returns a socket which listens on a specific port/address 43 * 44 * \param const Object* parentObject 45 * \param const string name 46 * \param unsigned int port 47 * \param const localAddress (defaults to any) 48 */ 49 virtual void Listen(const unsigned int port, 50 const std::string localAddress = "ANY") = 0; 51 51 52 /*! 53 * \brief Returns a socket on a new incoming connexion 54 * 55 * \param ConnectedSocket &listeningSocket 56 */ 57 virtual ConnectedSocket *Accept(Time timeout)=0; //should throw an exception if not a listening socket 52 /*! 53 * \brief Returns a socket on a new incoming connexion 54 * 55 * \param ConnectedSocket &listeningSocket 56 */ 57 virtual ConnectedSocket *Accept( 58 Time timeout) = 0; // should throw an exception if not a listening socket 58 59 59 /*! 60 * \brief Returns a socket connected to a distant host 61 * 62 * \param const Object* parentObject 63 * \param const string name 64 * \param unsigned int port 65 * \param const distantAddress 66 * \param timeout timeout (in milliseconds) 67 */ 68 virtual bool Connect(const unsigned int port,const std::string distantAddress,Time timeout)=0; 60 /*! 61 * \brief Returns a socket connected to a distant host 62 * 63 * \param const Object* parentObject 64 * \param const string name 65 * \param unsigned int port 66 * \param const distantAddress 67 * \param timeout timeout (in milliseconds) 68 */ 69 virtual bool Connect(const unsigned int port, 70 const std::string distantAddress, Time timeout) = 0; 69 71 70 /*! 71 * \brief Send a message 72 * 73 * \param message message 74 * \param message_len message length 75 * \param timeout timeout (in milliseconds) 76 */ 77 virtual ssize_t SendMessage(const char* message,size_t message_len,Time timeout)=0; 72 /*! 73 * \brief Send a message 74 * 75 * \param message message 76 * \param message_len message length 77 * \param timeout timeout (in milliseconds) 78 */ 79 virtual ssize_t SendMessage(const char *message, size_t message_len, 80 Time timeout) = 0; 78 81 79 80 81 82 83 84 85 86 87 88 89 90 virtual ssize_t RecvMessage(char* buf,size_t buf_len,Time timeout)=0;82 /*! 83 * \brief Receive a message 84 * 85 * Receive a message and wait up to timeout. \n 86 * 87 * \param buf buffer to put the message 88 * \param buf_len buffer length 89 * \param timeout timeout (in milliseconds) 90 * 91 * \return size of the received message 92 */ 93 virtual ssize_t RecvMessage(char *buf, size_t buf_len, Time timeout) = 0; 91 94 92 93 uint16_t ReadUInt16(Time const&timeout);94 void WriteUInt16(uint16_t const& data,Time const&timeout);95 uint32_t ReadUInt32(Time const&timeout);96 void WriteUInt32(uint32_t const& data,Time const&timeout);95 std::string ReadString(const size_t &stringLength, Time timeout); 96 uint16_t ReadUInt16(Time const &timeout); 97 void WriteUInt16(uint16_t const &data, Time const &timeout); 98 uint32_t ReadUInt32(Time const &timeout); 99 void WriteUInt32(uint32_t const &data, Time const &timeout); 97 100 98 //!! See Socket.h for a more generic implementation of network/host endianness conversion 99 virtual uint16_t NetworkToHost16(uint16_t data)=0; 100 virtual uint16_t HostToNetwork16(uint16_t data)=0; 101 virtual uint32_t NetworkToHost32(uint32_t data)=0; 102 virtual uint32_t HostToNetwork32(uint32_t data)=0; 101 //!! See Socket.h for a more generic implementation of network/host endianness 102 //conversion 103 virtual uint16_t NetworkToHost16(uint16_t data) = 0; 104 virtual uint16_t HostToNetwork16(uint16_t data) = 0; 105 virtual uint32_t NetworkToHost32(uint32_t data) = 0; 106 virtual uint32_t HostToNetwork32(uint32_t data) = 0; 103 107 }; 104 108 -
trunk/lib/FlairCore/src/DataPlot.cpp
r2 r15 24 24 using std::string; 25 25 26 namespace flair 27 { 28 namespace gui 29 { 26 namespace flair { 27 namespace gui { 30 28 31 29 using namespace core; 32 30 33 void RGBFromColor(DataPlot::Color_t color,uint8_t &r,uint8_t &g,uint8_t &b) 34 { 35 switch(color) 36 { 37 case DataPlot::Red: 38 r=255; 39 g=0; 40 b=0; 41 break; 42 case DataPlot::Blue: 43 r=0; 44 g=0; 45 b=255; 46 break; 47 case DataPlot::Green: 48 r=0; 49 g=255; 50 b=0; 51 break; 52 case DataPlot::Yellow: 53 r=255; 54 g=255; 55 b=0; 56 break; 57 case DataPlot::Black: 58 r=0; 59 g=0; 60 b=0; 61 break; 62 case DataPlot::White: 63 r=255; 64 g=255; 65 b=255; 66 break; 67 } 31 void RGBFromColor(DataPlot::Color_t color, uint8_t &r, uint8_t &g, uint8_t &b) { 32 switch (color) { 33 case DataPlot::Red: 34 r = 255; 35 g = 0; 36 b = 0; 37 break; 38 case DataPlot::Blue: 39 r = 0; 40 g = 0; 41 b = 255; 42 break; 43 case DataPlot::Green: 44 r = 0; 45 g = 255; 46 b = 0; 47 break; 48 case DataPlot::Yellow: 49 r = 255; 50 g = 255; 51 b = 0; 52 break; 53 case DataPlot::Black: 54 r = 0; 55 g = 0; 56 b = 0; 57 break; 58 case DataPlot::White: 59 r = 255; 60 g = 255; 61 b = 255; 62 break; 63 } 68 64 } 69 65 70 DataPlot::DataPlot(const LayoutPosition * position,string name,string type):SendData(position,name,type)71 {72 pimpl_=new DataPlot_impl();66 DataPlot::DataPlot(const LayoutPosition *position, string name, string type) 67 : SendData(position, name, type) { 68 pimpl_ = new DataPlot_impl(); 73 69 } 74 70 75 DataPlot::~DataPlot() 76 { 77 delete pimpl_; 71 DataPlot::~DataPlot() { delete pimpl_; } 72 73 void DataPlot::AddDataToSend(const IODataElement *element) { 74 pimpl_->tosend.push_back(element); 75 76 SetSendSize(SendSize() + element->Size()); 78 77 } 79 78 80 void DataPlot::AddDataToSend(const IODataElement *element) 81 { 82 pimpl_->tosend.push_back(element); 79 void DataPlot::CopyDatas(char *buf) const { 80 int buf_size = 0; 83 81 84 SetSendSize(SendSize()+element->Size()); 85 } 86 87 void DataPlot::CopyDatas(char* buf) const { 88 int buf_size=0; 89 90 for(size_t i=0;i<pimpl_->tosend.size();i++) { 91 pimpl_->tosend[i]->CopyData(buf+buf_size); 92 buf_size+=pimpl_->tosend[i]->Size(); 93 } 82 for (size_t i = 0; i < pimpl_->tosend.size(); i++) { 83 pimpl_->tosend[i]->CopyData(buf + buf_size); 84 buf_size += pimpl_->tosend[i]->Size(); 85 } 94 86 } 95 87 -
trunk/lib/FlairCore/src/DataPlot.h
r2 r15 18 18 class DataPlot_impl; 19 19 20 namespace flair 21 { 22 namespace core 23 { 24 class IODataElement; 25 } 20 namespace flair { 21 namespace core { 22 class IODataElement; 26 23 } 27 namespace flair 28 { 29 namespace gui 30 { 31 class LayoutPosition; 24 } 25 namespace flair { 26 namespace gui { 27 class LayoutPosition; 32 28 33 /*! \class DataPlot 34 * 35 * \brief Abstract class to display plots on ground station 36 * 37 */ 38 class DataPlot: public SendData 39 { 40 public: 41 /*! 42 \enum Color_t 43 \brief Types of colors 44 */ 45 typedef enum { 46 Red/*! red */, 47 Blue/*! blue */, 48 Green/*! green */, 49 Yellow/*! yellow */, 50 Black/*! black */, 51 White/*! white */, 52 } Color_t; 29 /*! \class DataPlot 30 * 31 * \brief Abstract class to display plots on ground station 32 * 33 */ 34 class DataPlot : public SendData { 35 public: 36 /*! 37 \enum Color_t 38 \brief Types of colors 39 */ 40 typedef enum { 41 Red /*! red */, 42 Blue /*! blue */, 43 Green /*! green */, 44 Yellow /*! yellow */, 45 Black /*! black */, 46 White /*! white */, 47 } Color_t; 53 48 54 55 56 57 58 59 60 61 62 63 64 65 66 DataPlot(const LayoutPosition* position,std::string name,std::string type);49 /*! 50 * \brief Constructor 51 * 52 * Type must agree with predifined (hard coded) types 53 * in ground station code. After calling this constructor, 54 * position will be deleted as it is no longer usefull. 55 * The DataPlot will automatically be child of position->getLayout() Layout. 56 * 57 * \param position position 58 * \param name nom 59 * \param type type 60 */ 61 DataPlot(const LayoutPosition *position, std::string name, std::string type); 67 62 68 69 70 71 72 63 /*! 64 * \brief Destructor 65 * 66 */ 67 ~DataPlot(); 73 68 74 75 76 77 78 79 80 81 82 69 protected: 70 /*! 71 * \brief Add an IODataElement to the plot. 72 * 73 * This method registers element for sending. 74 * 75 * \param element element to plot 76 */ 77 void AddDataToSend(const core::IODataElement *element); 83 78 84 85 86 87 88 89 90 91 92 void CopyDatas(char*buf) const;79 private: 80 /*! 81 * \brief Copy datas to specified buffer 82 * 83 * Reimplemented from SendData. 84 * 85 * \param buf output buffer 86 */ 87 void CopyDatas(char *buf) const; 93 88 94 95 96 97 98 99 89 /*! 90 * \brief Extra Xml event 91 * 92 * Reimplemented from SendData. 93 */ 94 void ExtraXmlEvent(void){}; 100 95 101 class DataPlot_impl*pimpl_;102 96 class DataPlot_impl *pimpl_; 97 }; 103 98 104 105 106 107 108 109 110 111 112 113 void RGBFromColor(DataPlot::Color_t color,uint8_t &r,uint8_t &g,uint8_t &b);99 /*! 100 * \brief Get RGB components from color type 101 * 102 * 103 * \param color input color 104 * \param r output component 105 * \param g output component 106 * \param b output component 107 */ 108 void RGBFromColor(DataPlot::Color_t color, uint8_t &r, uint8_t &g, uint8_t &b); 114 109 115 110 } // end namespace gui -
trunk/lib/FlairCore/src/DataPlot1D.cpp
r2 r15 23 23 using std::string; 24 24 25 namespace flair 26 { 27 namespace gui 28 { 25 namespace flair { 26 namespace gui { 29 27 30 28 using namespace core; 31 29 32 DataPlot1D::DataPlot1D(const LayoutPosition* position,string name,float ymin,float ymax):DataPlot(position,name,"DataPlot1D") 33 { 34 SetVolatileXmlProp("min",ymin); 35 SetVolatileXmlProp("max",ymax); 36 SendXml(); 30 DataPlot1D::DataPlot1D(const LayoutPosition *position, string name, float ymin, 31 float ymax) 32 : DataPlot(position, name, "DataPlot1D") { 33 SetVolatileXmlProp("min", ymin); 34 SetVolatileXmlProp("max", ymax); 35 SendXml(); 37 36 } 38 37 39 DataPlot1D::~DataPlot1D() { 38 DataPlot1D::~DataPlot1D() {} 39 40 void DataPlot1D::AddCurve(const core::IODataElement *element, uint8_t r, 41 uint8_t g, uint8_t b, string curve_name) { 42 if (curve_name != "") { 43 SetVolatileXmlProp("curve", curve_name); 44 } else { 45 SetVolatileXmlProp("curve", element->Parent()->ObjectName() + "\\" + 46 element->ObjectName()); 47 } 48 SetVolatileXmlProp("type", element->GetDataType().GetDescription()); 49 SetVolatileXmlProp("r", r); 50 SetVolatileXmlProp("g", g); 51 SetVolatileXmlProp("b", b); 52 53 SendXml(); 54 55 // save data information 56 AddDataToSend(element); 40 57 } 41 58 42 void DataPlot1D::AddCurve(const core::IODataElement* element,uint8_t r,uint8_t g,uint8_t b,string curve_name) 43 { 44 if(curve_name!="") 45 { 46 SetVolatileXmlProp("curve",curve_name); 47 } 48 else 49 { 50 SetVolatileXmlProp("curve",element->Parent()->ObjectName() + "\\" +element->ObjectName()); 51 } 52 SetVolatileXmlProp("type",element->GetDataType().GetDescription()); 53 SetVolatileXmlProp("r",r); 54 SetVolatileXmlProp("g",g); 55 SetVolatileXmlProp("b",b); 56 57 SendXml(); 58 59 //save data information 60 AddDataToSend(element); 61 } 62 63 void DataPlot1D::AddCurve(const core::IODataElement* element,Color_t color,string curve_name) { 64 uint8_t r,g,b; 65 RGBFromColor(color,r,g,b); 66 AddCurve(element,r,g,b,curve_name); 59 void DataPlot1D::AddCurve(const core::IODataElement *element, Color_t color, 60 string curve_name) { 61 uint8_t r, g, b; 62 RGBFromColor(color, r, g, b); 63 AddCurve(element, r, g, b, curve_name); 67 64 } 68 65 -
trunk/lib/FlairCore/src/DataPlot1D.h
r2 r15 17 17 #include <stdint.h> 18 18 19 namespace flair 20 { 21 namespace core 22 { 23 class IODataElement; 19 namespace flair { 20 namespace core { 21 class IODataElement; 24 22 } 25 23 26 namespace gui 27 { 24 namespace gui { 28 25 29 26 class LayoutPosition; 30 27 31 32 33 34 35 36 class DataPlot1D: private DataPlot 37 { 38 public:39 /*!40 * \brief Constructor41 *42 * Construct a 1D plot at given position.43 *44 * \param position position to display the plot45 * \param name name46 * \param ymin default yminof the plot47 * \param ymax default ymax of the plot48 */49 DataPlot1D(const LayoutPosition* position,std::string name,float ymin,float ymax);28 /*! \class DataPlot1D 29 * 30 * \brief Class displaying a 1D plot on the ground station 31 * 32 */ 33 class DataPlot1D : private DataPlot { 34 public: 35 /*! 36 * \brief Constructor 37 * 38 * Construct a 1D plot at given position. 39 * 40 * \param position position to display the plot 41 * \param name name 42 * \param ymin default ymin of the plot 43 * \param ymax default ymax of the plot 44 */ 45 DataPlot1D(const LayoutPosition *position, std::string name, float ymin, 46 float ymax); 50 47 51 52 53 54 55 48 /*! 49 * \brief Destructor 50 * 51 */ 52 ~DataPlot1D(); 56 53 57 /*! 58 * \brief Add a curve from an IODataElement to the plot. 59 * 60 * Curve's color can be selected by its name. 61 * 62 * \param element element to plot 63 * \param color color of the curve 64 * \param curve_name name of the curve for the legend, if unspecified, element->ObjectName() will be used 65 */ 66 void AddCurve(const core::IODataElement* element,Color_t color,std::string curve_name=""); 54 /*! 55 * \brief Add a curve from an IODataElement to the plot. 56 * 57 * Curve's color can be selected by its name. 58 * 59 * \param element element to plot 60 * \param color color of the curve 61 * \param curve_name name of the curve for the legend, if unspecified, 62 *element->ObjectName() will be used 63 */ 64 void AddCurve(const core::IODataElement *element, Color_t color, 65 std::string curve_name = ""); 67 66 68 /*! 69 * \brief Add a curve from an IODataElement to the plot. 70 * 71 * Curve's color can be selected by its RGB components. 72 * 73 * \param element element to plot 74 * \param r red component of the curve 75 * \param g green component of the curve 76 * \param b blue component of the curve 77 * \param curve_name name of the curve for the legend, if unspecified, element->ObjectName() will be used 78 */ 79 void AddCurve(const core::IODataElement* element,uint8_t r=255,uint8_t g=0,uint8_t b=0,std::string curve_name=""); 80 }; 67 /*! 68 * \brief Add a curve from an IODataElement to the plot. 69 * 70 * Curve's color can be selected by its RGB components. 71 * 72 * \param element element to plot 73 * \param r red component of the curve 74 * \param g green component of the curve 75 * \param b blue component of the curve 76 * \param curve_name name of the curve for the legend, if unspecified, 77 *element->ObjectName() will be used 78 */ 79 void AddCurve(const core::IODataElement *element, uint8_t r = 255, 80 uint8_t g = 0, uint8_t b = 0, std::string curve_name = ""); 81 }; 81 82 82 83 } // end namespace gui -
trunk/lib/FlairCore/src/DataPlot2D.cpp
r2 r15 23 23 using std::string; 24 24 25 namespace flair { namespace gui { 25 namespace flair { 26 namespace gui { 26 27 27 DataPlot2D::DataPlot2D(const LayoutPosition* position,string name,string x_name,float xmin,float xmax,string y_name,float ymin,float ymax):DataPlot(position,name,"DataPlot2D") { 28 SetVolatileXmlProp("xmin",xmin); 29 SetVolatileXmlProp("xmax",xmax); 30 SetVolatileXmlProp("ymin",ymin); 31 SetVolatileXmlProp("ymax",ymax); 32 SetVolatileXmlProp("x_name",x_name); 33 SetVolatileXmlProp("y_name",y_name); 34 SendXml(); 28 DataPlot2D::DataPlot2D(const LayoutPosition *position, string name, 29 string x_name, float xmin, float xmax, string y_name, 30 float ymin, float ymax) 31 : DataPlot(position, name, "DataPlot2D") { 32 SetVolatileXmlProp("xmin", xmin); 33 SetVolatileXmlProp("xmax", xmax); 34 SetVolatileXmlProp("ymin", ymin); 35 SetVolatileXmlProp("ymax", ymax); 36 SetVolatileXmlProp("x_name", x_name); 37 SetVolatileXmlProp("y_name", y_name); 38 SendXml(); 35 39 } 36 40 37 41 DataPlot2D::~DataPlot2D() {} 38 42 39 void DataPlot2D::AddCurve(const core::IODataElement* x_element,const core::IODataElement* y_element,Color_t color,string curve_name) { 40 uint8_t r,g,b; 41 RGBFromColor(color,r,g,b); 42 AddCurve(x_element,y_element,r,g,b,curve_name); 43 void DataPlot2D::AddCurve(const core::IODataElement *x_element, 44 const core::IODataElement *y_element, Color_t color, 45 string curve_name) { 46 uint8_t r, g, b; 47 RGBFromColor(color, r, g, b); 48 AddCurve(x_element, y_element, r, g, b, curve_name); 43 49 } 44 50 45 void DataPlot2D::AddCurve(const core::IODataElement* x_element,const core::IODataElement* y_element,uint8_t r,uint8_t g,uint8_t b,string curve_name) { 46 if(typeid(x_element->GetDataType())!=typeid(y_element->GetDataType())) { 47 Err("x_element type is different from y_element type (%i-%i)\n",x_element->GetDataType().GetDescription().c_str(),y_element->GetDataType().GetDescription().c_str()); 48 //gerer erreur 49 return; 50 } 51 void DataPlot2D::AddCurve(const core::IODataElement *x_element, 52 const core::IODataElement *y_element, uint8_t r, 53 uint8_t g, uint8_t b, string curve_name) { 54 if (typeid(x_element->GetDataType()) != typeid(y_element->GetDataType())) { 55 Err("x_element type is different from y_element type (%i-%i)\n", 56 x_element->GetDataType().GetDescription().c_str(), 57 y_element->GetDataType().GetDescription().c_str()); 58 // gerer erreur 59 return; 60 } 51 61 52 if(curve_name!="") { 53 SetVolatileXmlProp("curve",curve_name); 54 } else { 55 SetVolatileXmlProp("curve",x_element->Parent()->ObjectName() + "\\" +x_element->ObjectName()+ "\\" +y_element->ObjectName()); 56 } 57 SetVolatileXmlProp("type",x_element->GetDataType().GetDescription()); 58 SetVolatileXmlProp("r",r); 59 SetVolatileXmlProp("g",g); 60 SetVolatileXmlProp("b",b); 62 if (curve_name != "") { 63 SetVolatileXmlProp("curve", curve_name); 64 } else { 65 SetVolatileXmlProp("curve", x_element->Parent()->ObjectName() + "\\" + 66 x_element->ObjectName() + "\\" + 67 y_element->ObjectName()); 68 } 69 SetVolatileXmlProp("type", x_element->GetDataType().GetDescription()); 70 SetVolatileXmlProp("r", r); 71 SetVolatileXmlProp("g", g); 72 SetVolatileXmlProp("b", b); 61 73 62 74 SendXml(); 63 75 64 //save data information65 66 76 // save data information 77 AddDataToSend(x_element); 78 AddDataToSend(y_element); 67 79 } 68 80 -
trunk/lib/FlairCore/src/DataPlot2D.h
r2 r15 17 17 #include <stdint.h> 18 18 19 namespace flair 20 { 21 namespace core 22 { 23 class IODataElement; 24 } 19 namespace flair { 20 namespace core { 21 class IODataElement; 25 22 } 26 namespace flair 27 { 28 namespace gui 29 { 23 } 24 namespace flair { 25 namespace gui { 30 26 31 27 class LayoutPosition; 32 28 33 /*! \class DataPlot2D 34 * 35 * \brief Class displaying a 2D plot on the ground station 36 * 37 */ 38 class DataPlot2D: private DataPlot 39 { 40 public: 41 /*! 42 * \brief Constructor 43 * 44 * Construct a 2D plot at given place position. 45 * 46 * \param position position to display the plot 47 * \param name name 48 * \param x_name name of x axis 49 * \param xmin default xmin of the plot 50 * \param xmax default xmax of the plot 51 * \param y_name name of y axis 52 * \param ymin default ymin of the plot 53 * \param ymax default ymax of the plot 54 */ 55 DataPlot2D(const LayoutPosition* position,std::string name,std::string x_name,float xmin,float xmax,std::string y_name,float ymin,float ymax); 29 /*! \class DataPlot2D 30 * 31 * \brief Class displaying a 2D plot on the ground station 32 * 33 */ 34 class DataPlot2D : private DataPlot { 35 public: 36 /*! 37 * \brief Constructor 38 * 39 * Construct a 2D plot at given place position. 40 * 41 * \param position position to display the plot 42 * \param name name 43 * \param x_name name of x axis 44 * \param xmin default xmin of the plot 45 * \param xmax default xmax of the plot 46 * \param y_name name of y axis 47 * \param ymin default ymin of the plot 48 * \param ymax default ymax of the plot 49 */ 50 DataPlot2D(const LayoutPosition *position, std::string name, 51 std::string x_name, float xmin, float xmax, std::string y_name, 52 float ymin, float ymax); 56 53 57 58 59 60 61 54 /*! 55 * \brief Destructor 56 * 57 */ 58 ~DataPlot2D(); 62 59 63 /*! 64 * \brief Add a curve from elements to the plot. 65 * 66 * Curve's color can be selected by its name. 67 * 68 * \param x_element element to plot for x coordinate 69 * \param y_element element to plot for y coordinate 70 * \param color color of the curve 71 * \param curve_name name of the curve ofr the legend 72 */ 73 void AddCurve(const core::IODataElement* x_element,const core::IODataElement* y_element,Color_t color,std::string curve_name=""); 60 /*! 61 * \brief Add a curve from elements to the plot. 62 * 63 * Curve's color can be selected by its name. 64 * 65 * \param x_element element to plot for x coordinate 66 * \param y_element element to plot for y coordinate 67 * \param color color of the curve 68 * \param curve_name name of the curve ofr the legend 69 */ 70 void AddCurve(const core::IODataElement *x_element, 71 const core::IODataElement *y_element, Color_t color, 72 std::string curve_name = ""); 74 73 75 /*! 76 * \brief Add a curve from elements to the plot. 77 * 78 * Curve's color can be selected by its RGB components. 79 * 80 * \param x_element element to plot for x coordinate 81 * \param y_element element to plot for y coordinate 82 * \param r red component of the curve 83 * \param g green component of the curve 84 * \param b blue component of the curve 85 * \param curve_name name of the curve ofr the legend 86 */ 87 void AddCurve(const core::IODataElement* x_element,const core::IODataElement* y_element,uint8_t r=255,uint8_t g=0,uint8_t b=0,std::string curve_name=""); 88 }; 74 /*! 75 * \brief Add a curve from elements to the plot. 76 * 77 * Curve's color can be selected by its RGB components. 78 * 79 * \param x_element element to plot for x coordinate 80 * \param y_element element to plot for y coordinate 81 * \param r red component of the curve 82 * \param g green component of the curve 83 * \param b blue component of the curve 84 * \param curve_name name of the curve ofr the legend 85 */ 86 void AddCurve(const core::IODataElement *x_element, 87 const core::IODataElement *y_element, uint8_t r = 255, 88 uint8_t g = 0, uint8_t b = 0, std::string curve_name = ""); 89 }; 89 90 90 91 } // end namespace gui -
trunk/lib/FlairCore/src/DataPlot_impl.cpp
r2 r15 18 18 #include "DataPlot_impl.h" 19 19 20 DataPlot_impl::DataPlot_impl() 21 { 22 } 20 DataPlot_impl::DataPlot_impl() {} 23 21 24 DataPlot_impl::~DataPlot_impl() 25 { 26 } 22 DataPlot_impl::~DataPlot_impl() {} -
trunk/lib/FlairCore/src/DoubleSpinBox.cpp
r2 r15 20 20 using std::string; 21 21 22 namespace flair 23 { 24 namespace gui 25 { 22 namespace flair { 23 namespace gui { 26 24 27 DoubleSpinBox::DoubleSpinBox(const LayoutPosition* position,string name,double min,double max,double step,int decimals,double default_value):Box(position,name,"DoubleSpinBox") 28 { 29 //update value from xml file 30 if(default_value<min) default_value=min; 31 if(default_value>max) default_value=max; 32 box_value=default_value; 25 DoubleSpinBox::DoubleSpinBox(const LayoutPosition *position, string name, 26 double min, double max, double step, int decimals, 27 double default_value) 28 : Box(position, name, "DoubleSpinBox") { 29 // update value from xml file 30 if (default_value < min) 31 default_value = min; 32 if (default_value > max) 33 default_value = max; 34 box_value = default_value; 33 35 34 SetVolatileXmlProp("min",min);35 SetVolatileXmlProp("max",max);36 SetVolatileXmlProp("step",step);37 SetVolatileXmlProp("decimals",decimals);38 GetPersistentXmlProp("value",box_value);39 SetPersistentXmlProp("value",box_value);36 SetVolatileXmlProp("min", min); 37 SetVolatileXmlProp("max", max); 38 SetVolatileXmlProp("step", step); 39 SetVolatileXmlProp("decimals", decimals); 40 GetPersistentXmlProp("value", box_value); 41 SetPersistentXmlProp("value", box_value); 40 42 41 43 SendXml(); 42 44 } 43 45 44 DoubleSpinBox::DoubleSpinBox(const LayoutPosition* position,string name,string suffix,double min,double max,double step,int decimals,double default_value):Box(position,name,"DoubleSpinBox") 45 { 46 //update value from xml file 47 if(default_value<min) default_value=min; 48 if(default_value>max) default_value=max; 49 box_value=default_value; 46 DoubleSpinBox::DoubleSpinBox(const LayoutPosition *position, string name, 47 string suffix, double min, double max, double step, 48 int decimals, double default_value) 49 : Box(position, name, "DoubleSpinBox") { 50 // update value from xml file 51 if (default_value < min) 52 default_value = min; 53 if (default_value > max) 54 default_value = max; 55 box_value = default_value; 50 56 51 SetVolatileXmlProp("suffix",suffix);52 SetVolatileXmlProp("min",min);53 SetVolatileXmlProp("max",max);54 SetVolatileXmlProp("step",step);55 SetVolatileXmlProp("decimals",decimals);56 GetPersistentXmlProp("value",box_value);57 SetPersistentXmlProp("value",box_value);57 SetVolatileXmlProp("suffix", suffix); 58 SetVolatileXmlProp("min", min); 59 SetVolatileXmlProp("max", max); 60 SetVolatileXmlProp("step", step); 61 SetVolatileXmlProp("decimals", decimals); 62 GetPersistentXmlProp("value", box_value); 63 SetPersistentXmlProp("value", box_value); 58 64 59 65 SendXml(); 60 66 } 61 67 62 DoubleSpinBox::~DoubleSpinBox() 63 { 68 DoubleSpinBox::~DoubleSpinBox() {} 64 69 70 double DoubleSpinBox::Value(void) const { 71 double tmp; 72 73 GetMutex(); 74 tmp = box_value; 75 ReleaseMutex(); 76 77 return tmp; 65 78 } 66 79 67 double DoubleSpinBox::Value(void) const 68 { 69 double tmp; 70 71 GetMutex(); 72 tmp=box_value; 73 ReleaseMutex(); 74 75 return tmp; 76 } 77 78 void DoubleSpinBox::XmlEvent(void) 79 { 80 GetMutex(); 81 if(GetPersistentXmlProp("value",box_value)) SetValueChanged(); 82 ReleaseMutex(); 80 void DoubleSpinBox::XmlEvent(void) { 81 GetMutex(); 82 if (GetPersistentXmlProp("value", box_value)) 83 SetValueChanged(); 84 ReleaseMutex(); 83 85 } 84 86 -
trunk/lib/FlairCore/src/DoubleSpinBox.h
r2 r15 16 16 #include <Box.h> 17 17 18 namespace flair 19 { 20 namespace gui 21 { 18 namespace flair { 19 namespace gui { 22 20 23 21 class Layout; 24 22 25 /*! \class DoubleSpinBox 26 * 27 * \brief Class displaying a QDoubleSpinBox on the ground station 28 * 29 */ 30 class DoubleSpinBox: public Box 31 { 32 public: 33 /*! 34 * \brief Constructor 35 * 36 * Construct a QDoubleSpinBox at given position. \n 37 * The QDoubleSpinBox is saturated to min and max values. 38 * 39 * \param position position to display the QDoubleSpinBox 40 * \param name name 41 * \param min minimum value 42 * \param max maximum value 43 * \param step step 44 * \param decimals number of decimals 45 * \param default_value default value if not in the xml config file 46 */ 47 DoubleSpinBox(const LayoutPosition* position,std::string name,double min,double max,double step,int decimals=2,double default_value=0); 23 /*! \class DoubleSpinBox 24 * 25 * \brief Class displaying a QDoubleSpinBox on the ground station 26 * 27 */ 28 class DoubleSpinBox : public Box { 29 public: 30 /*! 31 * \brief Constructor 32 * 33 * Construct a QDoubleSpinBox at given position. \n 34 * The QDoubleSpinBox is saturated to min and max values. 35 * 36 * \param position position to display the QDoubleSpinBox 37 * \param name name 38 * \param min minimum value 39 * \param max maximum value 40 * \param step step 41 * \param decimals number of decimals 42 * \param default_value default value if not in the xml config file 43 */ 44 DoubleSpinBox(const LayoutPosition *position, std::string name, double min, 45 double max, double step, int decimals = 2, 46 double default_value = 0); 48 47 49 /*! 50 * \brief Constructor 51 * 52 * Construct a QDoubleSpinBox at given position. \n 53 * The QDoubleSpinBox is saturated to min and max values. 54 * 55 * \param position position to display the QDoubleSpinBox 56 * \param name name 57 * \param suffix suffix for the value (eg unit) 58 * \param min minimum value 59 * \param max maximum value 60 * \param step step 61 * \param decimals number of decimals 62 * \param default_value default value if not in the xml config file 63 */ 64 DoubleSpinBox(const LayoutPosition* position,std::string name,std::string suffix,double min,double max,double step,int decimals=2,double default_value=0); 48 /*! 49 * \brief Constructor 50 * 51 * Construct a QDoubleSpinBox at given position. \n 52 * The QDoubleSpinBox is saturated to min and max values. 53 * 54 * \param position position to display the QDoubleSpinBox 55 * \param name name 56 * \param suffix suffix for the value (eg unit) 57 * \param min minimum value 58 * \param max maximum value 59 * \param step step 60 * \param decimals number of decimals 61 * \param default_value default value if not in the xml config file 62 */ 63 DoubleSpinBox(const LayoutPosition *position, std::string name, 64 std::string suffix, double min, double max, double step, 65 int decimals = 2, double default_value = 0); 65 66 66 67 68 69 70 67 /*! 68 * \brief Destructor 69 * 70 */ 71 ~DoubleSpinBox(); 71 72 72 73 74 75 76 77 73 /*! 74 * \brief Value 75 * 76 * \return value 77 */ 78 double Value(void) const; 78 79 79 80 81 82 83 84 85 86 80 private: 81 /*! 82 * \brief XmlEvent from ground station 83 * 84 * Reimplemented from Widget. 85 * 86 */ 87 void XmlEvent(void); 87 88 88 89 89 double box_value; 90 }; 90 91 91 92 } // end namespace gui -
trunk/lib/FlairCore/src/Euler.cpp
r2 r15 23 23 #define PI_D ((double)3.14159265358979323846) 24 24 25 namespace flair 26 { 27 namespace core 28 { 25 namespace flair { 26 namespace core { 29 27 30 Euler::Euler(float inRoll, float inPitch,float inYaw): roll(inRoll), pitch(inPitch),yaw(inYaw) {31 }28 Euler::Euler(float inRoll, float inPitch, float inYaw) 29 : roll(inRoll), pitch(inPitch), yaw(inYaw) {} 32 30 33 Euler::~Euler() { 31 Euler::~Euler() {} 34 32 35 } 36 37 Euler& Euler::operator=(const Euler &euler) { 38 roll=euler.roll; 39 pitch=euler.pitch; 40 yaw=euler.yaw; 41 return (*this); 33 Euler &Euler::operator=(const Euler &euler) { 34 roll = euler.roll; 35 pitch = euler.pitch; 36 yaw = euler.yaw; 37 return (*this); 42 38 } 43 39 /* … … 85 81 */ 86 82 void Euler::ToQuaternion(Quaternion &quaternion) const { 87 quaternion.q0=cos(yaw/2)*cos(pitch/2)*cos(roll/2)88 +sin(yaw/2)*sin(pitch/2)*sin(roll/2);83 quaternion.q0 = cos(yaw / 2) * cos(pitch / 2) * cos(roll / 2) + 84 sin(yaw / 2) * sin(pitch / 2) * sin(roll / 2); 89 85 90 quaternion.q1=cos(yaw/2)*cos(pitch/2)*sin(roll/2)91 -sin(yaw/2)*sin(pitch/2)*cos(roll/2);86 quaternion.q1 = cos(yaw / 2) * cos(pitch / 2) * sin(roll / 2) - 87 sin(yaw / 2) * sin(pitch / 2) * cos(roll / 2); 92 88 93 quaternion.q2=cos(yaw/2)*sin(pitch/2)*cos(roll/2)94 +sin(yaw/2)*cos(pitch/2)*sin(roll/2);89 quaternion.q2 = cos(yaw / 2) * sin(pitch / 2) * cos(roll / 2) + 90 sin(yaw / 2) * cos(pitch / 2) * sin(roll / 2); 95 91 96 quaternion.q3=sin(yaw/2)*cos(pitch/2)*cos(roll/2)97 -cos(yaw/2)*sin(pitch/2)*sin(roll/2);92 quaternion.q3 = sin(yaw / 2) * cos(pitch / 2) * cos(roll / 2) - 93 cos(yaw / 2) * sin(pitch / 2) * sin(roll / 2); 98 94 } 99 95 100 96 Quaternion Euler::ToQuaternion(void) const { 101 102 103 97 Quaternion quaternion; 98 ToQuaternion(quaternion); 99 return quaternion; 104 100 } 105 101 106 float Euler::ToDegree(float radianValue) { 107 return radianValue*180.0f/PI; 108 } 102 float Euler::ToDegree(float radianValue) { return radianValue * 180.0f / PI; } 109 103 110 float Euler::ToRadian(float degreeValue) { 111 return degreeValue/180.0f*PI; 112 } 104 float Euler::ToRadian(float degreeValue) { return degreeValue / 180.0f * PI; } 113 105 114 106 float Euler::YawDistanceFrom(float angle) const { 115 float rot1,rot2; 116 if(angle > yaw ) { 117 rot1=angle-yaw; 118 rot2=2*PI-angle+yaw; 119 } else { 120 rot1=2*PI+angle-yaw; 121 rot2=yaw-angle; 122 } 123 if(rot2<rot1) rot1=-rot2; 124 rot1=-rot1;//pour avoir rot1=yaw-angle 107 float rot1, rot2; 108 if (angle > yaw) { 109 rot1 = angle - yaw; 110 rot2 = 2 * PI - angle + yaw; 111 } else { 112 rot1 = 2 * PI + angle - yaw; 113 rot2 = yaw - angle; 114 } 115 if (rot2 < rot1) 116 rot1 = -rot2; 117 rot1 = -rot1; // pour avoir rot1=yaw-angle 125 118 126 119 return rot1; 127 120 } 128 121 -
trunk/lib/FlairCore/src/Euler.h
r2 r15 14 14 #define EULER_H 15 15 16 namespace flair { namespace core { 17 class Quaternion; 16 namespace flair { 17 namespace core { 18 class Quaternion; 18 19 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 Euler(float roll=0,float pitch=0,float yaw=0);20 /*! \class Euler 21 * 22 * \brief Class defining euler angles 23 * 24 * Euler angles are expressed in radians. 25 * 26 */ 27 class Euler { 28 public: 29 /*! 30 * \brief Constructor 31 * 32 * Construct euler angles using specified values. 33 * 34 * \param roll roll value 35 * \param pitch pitch value 36 * \param yaw yaw value 37 */ 38 Euler(float roll = 0, float pitch = 0, float yaw = 0); 38 39 39 40 41 42 43 40 /*! 41 * \brief Destructor 42 * 43 */ 44 ~Euler(); 44 45 45 46 47 48 49 50 //todo: passer par un quaternion51 //void RotateX(float value);46 /*! 47 * \brief x axis rotation 48 * 49 * \param value rotation value in radians 50 */ 51 // todo: passer par un quaternion 52 // void RotateX(float value); 52 53 53 54 55 56 57 58 //void RotateXDeg(float value);54 /*! 55 * \brief x axis rotation 56 * 57 * \param value rotation value in degrees 58 */ 59 // void RotateXDeg(float value); 59 60 60 61 62 63 64 65 //void RotateY(float value);61 /*! 62 * \brief y axis rotation 63 * 64 * \param value rotation value in radians 65 */ 66 // void RotateY(float value); 66 67 67 68 69 70 71 72 //void RotateYDeg(float value);68 /*! 69 * \brief y axis rotation 70 * 71 * \param value rotation value in degrees 72 */ 73 // void RotateYDeg(float value); 73 74 74 75 76 77 78 79 //void RotateZ(float value);75 /*! 76 * \brief z axis rotation 77 * 78 * \param value rotation value in radians 79 */ 80 // void RotateZ(float value); 80 81 81 82 83 84 85 86 //void RotateZDeg(float value);82 /*! 83 * \brief z axis rotation 84 * 85 * \param value rotation value in degrees 86 */ 87 // void RotateZDeg(float value); 87 88 88 89 90 91 92 93 89 /*! 90 * \brief Convert to quaternion 91 * 92 * \param quaternion output quaternion 93 */ 94 void ToQuaternion(Quaternion &quaternion) const; 94 95 95 96 97 98 99 100 101 102 103 104 105 106 107 108 96 /*! 97 * \brief Convert to quaternion 98 * 99 * \return quaternion 100 */ 101 Quaternion ToQuaternion(void) const; 102 /*! 103 * \brief Convert from radian to degree 104 * 105 * \param radianValue value in radian 106 * 107 * \return value in degree 108 */ 109 static float ToDegree(float radianValue); 109 110 110 111 112 113 114 115 116 117 111 /*! 112 * \brief Convert from degree to radian 113 * 114 * \param degreeValue value in degree 115 * 116 * \return value in radian 117 */ 118 static float ToRadian(float degreeValue); 118 119 119 120 121 122 123 124 125 126 127 128 120 /*! 121 * \brief Compute yaw distance 122 * 123 * Compute yaw distance from given angle. This is the minimum distance. 124 * 125 * \param angle starting angle 126 * 127 * \return value distance in radian 128 */ 129 float YawDistanceFrom(float angle) const; 129 130 130 131 132 133 131 /*! 132 * \brief roll value 133 */ 134 float roll; 134 135 135 136 137 138 136 /*! 137 * \brief pitch value 138 */ 139 float pitch; 139 140 140 141 142 143 141 /*! 142 * \brief yaw value 143 */ 144 float yaw; 144 145 145 Euler& operator=(const Euler &euler); 146 147 }; 146 Euler &operator=(const Euler &euler); 147 }; 148 148 149 149 } // end namespace core -
trunk/lib/FlairCore/src/FrameworkManager.cpp
r2 r15 26 26 using namespace flair::gui; 27 27 28 namespace 29 { 30 flair::core::FrameworkManager* frameworkmanager=NULL; 28 namespace { 29 flair::core::FrameworkManager *frameworkmanager = NULL; 31 30 } 32 31 32 namespace flair { 33 namespace core { 33 34 34 namespace flair 35 { 36 namespace core 37 { 35 FrameworkManager *getFrameworkManager(void) { return frameworkmanager; } 38 36 37 bool IsBigEndian(void) { 38 union { 39 uint32_t i; 40 char c[4]; 41 } bint = {0x01020304}; 39 42 40 FrameworkManager* getFrameworkManager(void) { 41 return frameworkmanager; 43 if (bint.c[0] == 1) { 44 return true; 45 } else { 46 return false; 47 } 42 48 } 43 49 44 bool IsBigEndian(void) { 45 union { 46 uint32_t i; 47 char c[4]; 48 } bint = {0x01020304}; 50 FrameworkManager::FrameworkManager(string name) 51 : Object(NULL, name, XML_ROOT_TYPE) { 52 if (frameworkmanager != NULL) { 53 Err("FrameworkManager should be instanced only one time\n"); 54 return; 55 } 49 56 50 if(bint.c[0] == 1) { 51 return true; 52 } else { 53 return false; 54 } 55 } 56 57 FrameworkManager::FrameworkManager(string name): Object(NULL,name,XML_ROOT_TYPE) 58 { 59 if(frameworkmanager!=NULL) { 60 Err("FrameworkManager should be instanced only one time\n"); 61 return; 62 } 63 64 Printf(SVN_REV); 65 frameworkmanager=this; 66 pimpl_=new FrameworkManager_impl(this,name); 57 Printf(SVN_REV); 58 frameworkmanager = this; 59 pimpl_ = new FrameworkManager_impl(this, name); 67 60 } 68 61 69 62 FrameworkManager::~FrameworkManager() { 70 //Printf("destruction FrameworkManager\n");63 // Printf("destruction FrameworkManager\n"); 71 64 72 //stop ui_com thread (which sends datas for graphs), we do not need it as graphs will be destroyed 73 if(getUiCom()!=NULL) { 74 getUiCom()->SafeStop(); 75 getUiCom()->Join(); 65 // stop ui_com thread (which sends datas for graphs), we do not need it as 66 // graphs will be destroyed 67 if (getUiCom() != NULL) { 68 getUiCom()->SafeStop(); 69 getUiCom()->Join(); 70 } 71 72 // we start by deleting threads (except pimpl which must be deleted at last) 73 // (before deleting objects that could be used by the threads) 74 // Printf("delete Threads\n"); 75 for (vector<const Object *>::iterator it = Childs()->begin(); 76 it < Childs()->end(); it++) { 77 // Printf("child %i %s 78 // %s\n",Childs()->size(),(*it)->ObjectName().c_str(),(*it)->ObjectType().c_str()); 79 if ((*it)->ObjectType() == "Thread") { 80 if (*it != pimpl_) { 81 // Printf("del\n"); 82 delete (Thread *)(*it); 83 // Childs() vector has been modified, we start from beginning again 84 // it will be incremented by the loop, so in fact we start again at 85 // begin()+1 86 // it is not a problem since begin is pimpl 87 it = Childs()->begin(); 88 // Printf("del ok\n"); 89 } 76 90 } 91 } 77 92 78 //we start by deleting threads (except pimpl which must be deleted at last) 79 // (before deleting objects that could be used by the threads) 80 //Printf("delete Threads\n"); 81 for(vector<const Object*>::iterator it=Childs()->begin() ; it < Childs()->end(); it++ ) { 82 //Printf("child %i %s %s\n",Childs()->size(),(*it)->ObjectName().c_str(),(*it)->ObjectType().c_str()); 83 if((*it)->ObjectType()=="Thread") { 84 if(*it!=pimpl_) { 85 //Printf("del\n"); 86 delete (Thread*)(*it); 87 //Childs() vector has been modified, we start from beginning again 88 //it will be incremented by the loop, so in fact we start again at begin()+1 89 //it is not a problem since begin is pimpl 90 it=Childs()->begin(); 91 //Printf("del ok\n"); 92 } 93 } 93 // next we delete IODevice 94 // (before deleting objects that could be used by the IODevice) 95 // Printf("delete IODevices\n"); 96 for (vector<const Object *>::iterator it = Childs()->begin(); 97 it < Childs()->end(); it++) { 98 // Printf("child %i %s 99 // %s\n",Childs()->size(),(*it)->ObjectName().c_str(),(*it)->ObjectType().c_str()); 100 if ((*it)->ObjectType() == "IODevice") { 101 // Printf("del\n"); 102 delete (IODevice *)*it; 103 // Printf("del ok\n"); 104 // Childs() vector has been modified, we start from beginning again 105 // it will be incremented by the loop, so in fact we start again at 106 // begin()+1 107 // it is not a problem since begin is pimpl (not an IODevice) 108 it = Childs()->begin(); 109 // Printf("del ok\n"); 94 110 } 111 } 95 112 96 //next we delete IODevice 97 // (before deleting objects that could be used by the IODevice) 98 //Printf("delete IODevices\n"); 99 for(vector<const Object*>::iterator it=Childs()->begin() ; it < Childs()->end(); it++ ) { 100 //Printf("child %i %s %s\n",Childs()->size(),(*it)->ObjectName().c_str(),(*it)->ObjectType().c_str()); 101 if((*it)->ObjectType()=="IODevice") { 102 //Printf("del\n"); 103 delete (IODevice*)*it; 104 //Printf("del ok\n"); 105 //Childs() vector has been modified, we start from beginning again 106 //it will be incremented by the loop, so in fact we start again at begin()+1 107 //it is not a problem since begin is pimpl (not an IODevice) 108 it=Childs()->begin(); 109 //Printf("del ok\n"); 110 } 111 } 113 // Printf("delete childs\n"); 114 // on efface les enfants en commencant par la fin 115 // le ui_com puis FrameworkManager_impl est détruit en dernier 116 // permet de garder l'uicom pour notifer des destructions 117 while (Childs()->size() != 0) { 118 // Printf("child %i %s 119 // %s\n",Childs()->size(),Childs()->back()->ObjectName().c_str(),Childs()->back()->ObjectType().c_str()); 120 if (Childs()->back() != NULL) 121 delete Childs()->back(); 122 } 112 123 113 //Printf("delete childs\n"); 114 //on efface les enfants en commencant par la fin 115 //le ui_com puis FrameworkManager_impl est détruit en dernier 116 //permet de garder l'uicom pour notifer des destructions 117 while(Childs()->size()!=0) { 118 //Printf("child %i %s %s\n",Childs()->size(),Childs()->back()->ObjectName().c_str(),Childs()->back()->ObjectType().c_str()); 119 if(Childs()->back()!=NULL) delete Childs()->back(); 120 } 124 // childs.clear(); 121 125 122 //childs.clear(); 123 124 //Printf("destruction FrameworkManager ok\n"); 126 // Printf("destruction FrameworkManager ok\n"); 125 127 } 126 128 127 void FrameworkManager::SetupConnection(string address,uint16_t port,size_t rcv_buf_size) { 128 pimpl_->SetupConnection(address,port,rcv_buf_size); 129 void FrameworkManager::SetupConnection(string address, uint16_t port, 130 size_t rcv_buf_size) { 131 pimpl_->SetupConnection(address, port, rcv_buf_size); 129 132 } 130 133 131 134 void FrameworkManager::SetupUserInterface(string xml_file) { 132 135 pimpl_->SetupUserInterface(xml_file); 133 136 } 134 137 135 gui::TabWidget *FrameworkManager::GetTabWidget(void) const {136 138 gui::TabWidget *FrameworkManager::GetTabWidget(void) const { 139 return pimpl_->tabwidget; 137 140 } 138 141 139 142 void FrameworkManager::UpdateSendData(const SendData *obj) { 140 if(getUiCom()!=NULL) getUiCom()->UpdateSendData(obj); 143 if (getUiCom() != NULL) 144 getUiCom()->UpdateSendData(obj); 141 145 } 142 146 143 147 void FrameworkManager::BlockCom(void) { 144 if(getUiCom()!=NULL) getUiCom()->Block(); 148 if (getUiCom() != NULL) 149 getUiCom()->Block(); 145 150 } 146 151 147 152 void FrameworkManager::UnBlockCom(void) { 148 if(getUiCom()!=NULL) getUiCom()->UnBlock(); 153 if (getUiCom() != NULL) 154 getUiCom()->UnBlock(); 149 155 } 150 156 151 157 bool FrameworkManager::ConnectionLost(void) const { 152 if(getUiCom()!=NULL) {153 return (getUiCom()->ConnectionLost()| pimpl_->connection_lost);154 155 156 158 if (getUiCom() != NULL) { 159 return (getUiCom()->ConnectionLost() | pimpl_->connection_lost); 160 } else { 161 return false; 162 } 157 163 } 158 164 159 165 void FrameworkManager::SetupLogger(string log_path) { 160 166 pimpl_->SetupLogger(log_path); 161 167 } 162 168 163 169 void FrameworkManager::AddDeviceToLog(IODevice *device) { 164 170 pimpl_->AddDeviceToLog(device); 165 171 } 166 172 167 void FrameworkManager::StartLog(void) { 168 pimpl_->StartLog(); 169 } 173 void FrameworkManager::StartLog(void) { pimpl_->StartLog(); } 170 174 171 void FrameworkManager::StopLog(void) { 172 pimpl_->StopLog(); 173 } 175 void FrameworkManager::StopLog(void) { pimpl_->StopLog(); } 174 176 175 bool FrameworkManager::IsLogging(void) const { 176 return pimpl_->is_logging; 177 } 177 bool FrameworkManager::IsLogging(void) const { return pimpl_->is_logging; } 178 178 179 179 void FrameworkManager::DisableErrorsDisplay(bool value) { 180 pimpl_->disable_errors=value;180 pimpl_->disable_errors = value; 181 181 } 182 182 183 183 bool FrameworkManager::IsDisplayingErrors(void) const { 184 184 return !(pimpl_->disable_errors); 185 185 } 186 186 -
trunk/lib/FlairCore/src/FrameworkManager.h
r2 r15 18 18 class FrameworkManager_impl; 19 19 20 namespace flair 21 { 22 namespace gui 23 { 24 class TabWidget; 25 class SendData; 26 } 20 namespace flair { 21 namespace gui { 22 class TabWidget; 23 class SendData; 27 24 } 28 namespace flair 29 { 30 namespace core 31 { 32 class IODevice; 33 34 /*! \class FrameworkManager 35 * 36 * \brief Main class of the Framework library 37 * 38 * This is the main class of the library. Only one instance of this class is allowed 39 * by program. Morevoer, its name must be unique if more than one program using this class 40 * is running on the same system (a control and a simlator for example). \n 41 * This class allows: \n 42 * -connexion with ground station, \n 43 * -creation of a QTabWidget on ground station, \n 44 * -handling of xml files, used for default values of Widgets, \n 45 * -logging of datas. 46 */ 47 class FrameworkManager: public Object 48 { 49 public: 50 /*! 51 * \brief Constructor 52 * 53 * Construct a FrameworkManager. \n 54 * Call SetupConnection method just after this constructor to setup the conection with a ground station. 55 * 56 * \param name name, must be unique 57 */ 58 FrameworkManager(std::string name); 59 60 /*! 61 * \brief Destructor 62 * 63 * Calling it will automatically destruct all childs. \n 64 * Destruction implies destruction of the QTabWidget on ground station. 65 * 66 */ 67 ~FrameworkManager(); 68 69 /*! 70 * \brief Setup the connection with ground station 71 * 72 * Call this method just after the constructor of this class. If this method is not called, the program will run headless. 73 * 74 * \param address address of ground station 75 * \param port port of ground station 76 * \param rcv_buf_size receive buffer size 77 */ 78 void SetupConnection(std::string address,uint16_t port,size_t rcv_buf_size=10000); 79 80 /*! 81 * \brief Setup the user interface 82 * 83 * If this method is called after SetupConnection, Widgets will be displayed in the ground station. 84 * Otherwise, it will run headless, but default values of Widgets will be taken from the xml file. 85 * 86 * \param xml_file xml file for default values of Widgets 87 */ 88 void SetupUserInterface(std::string xml_file); 89 90 /*! 91 * \brief Get TabWidget 92 * 93 * \return TabWidget 94 */ 95 gui::TabWidget* GetTabWidget(void) const; 96 97 /*! 98 * \brief Logger setup 99 * 100 * Setup path of log files. \n 101 * No logging will be performed if this method is not called. \n 102 * 103 * \param log_path path to store logs 104 */ 105 void SetupLogger(std::string log_path); 106 107 /*! 108 * \brief Add log element 109 * 110 * The added element will be automatically logged once 111 * logging started (see StartLog()). \n 112 * This element must define on its side the io_data 113 * to log, trough IODevice::SetDataToLog method. 114 * 115 * \param device IODevice to add 116 */ 117 void AddDeviceToLog(IODevice *device); 118 119 /*! 120 * \brief Start logging 121 * 122 * All IODevice added through AddDeviceToLog() method 123 * will automatically be logged. \n 124 * SetupLogger() must have been called before. 125 */ 126 void StartLog(void); 127 128 /*! 129 * \brief Stop logging 130 * 131 * Logs will automatically be sent to ground station. 132 */ 133 void StopLog(void); 134 135 /*! 136 * \brief Is logging? 137 * 138 * \return true if is logging 139 */ 140 bool IsLogging(void) const; 141 142 /*! 143 * \brief Notify that SendData's period has changed 144 * 145 * This funtion must be called when the period has changed. \n 146 * Normally, it occurs in the Widget::XmlEvent method. \n 147 * This method must be called with communication blocked (see BlockCom()). 148 * 149 * \param obj SendData which changed 150 */ 151 void UpdateSendData(const gui::SendData *obj); 152 153 /*! 154 * \brief Block communication 155 * 156 * This funtion blocks the communication beetween the program and ground station. \n 157 * It must be called before changing datas or parameters exchanged between the program 158 * and the ground station. 159 * 160 */ 161 void BlockCom(void); 162 163 /*! 164 * \brief Unblock communication 165 * 166 * This funtion unblocks the communication beetween the program and ground station. \n 167 * It must be called after changing datas or parameters exchanged between the program 168 * and the ground station. 169 * 170 */ 171 void UnBlockCom(void); 172 173 /*! 174 * \brief Is connection lost? 175 * 176 * Once this method returns true, it will never return false back. \n 177 * Note that this method return false if no connection is defined (see SetupConnection). 178 * 179 * \return true if connection with ground station is lost 180 */ 181 bool ConnectionLost(void) const; 182 183 /*! 184 * \brief Disable errors display 185 * 186 * Disable errors display, if you do not want to saturate console for exemple. 187 * By defaults errors disply is enabled. 188 * 189 * \param value true to disable errors display 190 */ 191 void DisableErrorsDisplay(bool value); 192 193 /*! 194 * \brief Is displaying errors? 195 * 196 * 197 * \return true if errors display is enabled 198 */ 199 bool IsDisplayingErrors(void) const; 200 201 private: 202 class FrameworkManager_impl* pimpl_; 203 }; 204 205 /*! 206 * \brief get FrameworkManager 207 * 208 * \return the FrameworkManager 209 */ 210 FrameworkManager* getFrameworkManager(void); 211 212 /*! 213 * \brief is big endian? 214 * 215 * \return true if big endian, false if little endian 216 */ 217 bool IsBigEndian(void); 25 } 26 namespace flair { 27 namespace core { 28 class IODevice; 29 30 /*! \class FrameworkManager 31 * 32 * \brief Main class of the Framework library 33 * 34 * This is the main class of the library. Only one instance of this class is 35 *allowed 36 * by program. Morevoer, its name must be unique if more than one program using 37 *this class 38 * is running on the same system (a control and a simlator for example). \n 39 * This class allows: \n 40 * -connexion with ground station, \n 41 * -creation of a QTabWidget on ground station, \n 42 * -handling of xml files, used for default values of Widgets, \n 43 * -logging of datas. 44 */ 45 class FrameworkManager : public Object { 46 public: 47 /*! 48 * \brief Constructor 49 * 50 * Construct a FrameworkManager. \n 51 * Call SetupConnection method just after this constructor to setup the 52 *conection with a ground station. 53 * 54 * \param name name, must be unique 55 */ 56 FrameworkManager(std::string name); 57 58 /*! 59 * \brief Destructor 60 * 61 * Calling it will automatically destruct all childs. \n 62 * Destruction implies destruction of the QTabWidget on ground station. 63 * 64 */ 65 ~FrameworkManager(); 66 67 /*! 68 * \brief Setup the connection with ground station 69 * 70 * Call this method just after the constructor of this class. If this method is 71 *not called, the program will run headless. 72 * 73 * \param address address of ground station 74 * \param port port of ground station 75 * \param rcv_buf_size receive buffer size 76 */ 77 void SetupConnection(std::string address, uint16_t port, 78 size_t rcv_buf_size = 10000); 79 80 /*! 81 * \brief Setup the user interface 82 * 83 * If this method is called after SetupConnection, Widgets will be displayed in 84 *the ground station. 85 * Otherwise, it will run headless, but default values of Widgets will be taken 86 *from the xml file. 87 * 88 * \param xml_file xml file for default values of Widgets 89 */ 90 void SetupUserInterface(std::string xml_file); 91 92 /*! 93 * \brief Get TabWidget 94 * 95 * \return TabWidget 96 */ 97 gui::TabWidget *GetTabWidget(void) const; 98 99 /*! 100 * \brief Logger setup 101 * 102 * Setup path of log files. \n 103 * No logging will be performed if this method is not called. \n 104 * 105 * \param log_path path to store logs 106 */ 107 void SetupLogger(std::string log_path); 108 109 /*! 110 * \brief Add log element 111 * 112 * The added element will be automatically logged once 113 * logging started (see StartLog()). \n 114 * This element must define on its side the io_data 115 * to log, trough IODevice::SetDataToLog method. 116 * 117 * \param device IODevice to add 118 */ 119 void AddDeviceToLog(IODevice *device); 120 121 /*! 122 * \brief Start logging 123 * 124 * All IODevice added through AddDeviceToLog() method 125 * will automatically be logged. \n 126 * SetupLogger() must have been called before. 127 */ 128 void StartLog(void); 129 130 /*! 131 * \brief Stop logging 132 * 133 * Logs will automatically be sent to ground station. 134 */ 135 void StopLog(void); 136 137 /*! 138 * \brief Is logging? 139 * 140 * \return true if is logging 141 */ 142 bool IsLogging(void) const; 143 144 /*! 145 * \brief Notify that SendData's period has changed 146 * 147 * This funtion must be called when the period has changed. \n 148 * Normally, it occurs in the Widget::XmlEvent method. \n 149 * This method must be called with communication blocked (see BlockCom()). 150 * 151 * \param obj SendData which changed 152 */ 153 void UpdateSendData(const gui::SendData *obj); 154 155 /*! 156 * \brief Block communication 157 * 158 * This funtion blocks the communication beetween the program and ground 159 *station. \n 160 * It must be called before changing datas or parameters exchanged between the 161 *program 162 * and the ground station. 163 * 164 */ 165 void BlockCom(void); 166 167 /*! 168 * \brief Unblock communication 169 * 170 * This funtion unblocks the communication beetween the program and ground 171 *station. \n 172 * It must be called after changing datas or parameters exchanged between the 173 *program 174 * and the ground station. 175 * 176 */ 177 void UnBlockCom(void); 178 179 /*! 180 * \brief Is connection lost? 181 * 182 * Once this method returns true, it will never return false back. \n 183 * Note that this method return false if no connection is defined (see 184 *SetupConnection). 185 * 186 * \return true if connection with ground station is lost 187 */ 188 bool ConnectionLost(void) const; 189 190 /*! 191 * \brief Disable errors display 192 * 193 * Disable errors display, if you do not want to saturate console for exemple. 194 * By defaults errors disply is enabled. 195 * 196 * \param value true to disable errors display 197 */ 198 void DisableErrorsDisplay(bool value); 199 200 /*! 201 * \brief Is displaying errors? 202 * 203 * 204 * \return true if errors display is enabled 205 */ 206 bool IsDisplayingErrors(void) const; 207 208 private: 209 class FrameworkManager_impl *pimpl_; 210 }; 211 212 /*! 213 * \brief get FrameworkManager 214 * 215 * \return the FrameworkManager 216 */ 217 FrameworkManager *getFrameworkManager(void); 218 219 /*! 220 * \brief is big endian? 221 * 222 * \return true if big endian, false if little endian 223 */ 224 bool IsBigEndian(void); 218 225 219 226 } // end namespace core -
trunk/lib/FlairCore/src/FrameworkManager_impl.cpp
r2 r15 43 43 using namespace flair::gui; 44 44 45 ui_com * FrameworkManager_impl::com=NULL;46 FrameworkManager_impl * FrameworkManager_impl::_this=NULL;45 ui_com *FrameworkManager_impl::com = NULL; 46 FrameworkManager_impl *FrameworkManager_impl::_this = NULL; 47 47 48 48 namespace { … … 50 50 51 51 #ifdef SIGDEBUG 52 static const char *reason_str[] = { 53 "undefined", 54 "received signal", 55 "invoked syscall", 56 "triggered fault", 57 "affected by priority inversion", 58 "missing mlockall", 59 "runaway thread", 60 }; 61 62 void warn_upon_switch(int sig, siginfo_t *si, void *context) { 63 unsigned int reason = si->si_value.sival_int; 64 void *bt[32]; 65 int nentries; 52 static const char *reason_str[] = { 53 "undefined", "received signal", "invoked syscall", "triggered fault", 54 "affected by priority inversion", "missing mlockall", "runaway thread", 55 }; 56 57 void warn_upon_switch(int sig, siginfo_t *si, void *context) { 58 unsigned int reason = si->si_value.sival_int; 59 void *bt[32]; 60 int nentries; 66 61 #ifdef SIGDEBUG_WATCHDOG 67 68 69 #endif 70 71 nentries = backtrace(bt,sizeof(bt) / sizeof(bt[0]));72 backtrace_symbols_fd(bt,nentries,fileno(stdout));73 74 #else //SIGDEBUG75 76 77 78 79 80 81 nentries = backtrace(bt,sizeof(bt) / sizeof(bt[0]));82 backtrace_symbols_fd(bt,nentries,fileno(stdout));83 84 #endif // SIGDEBUG62 printf("\nSIGDEBUG received, reason %d: %s\n", reason, 63 reason <= SIGDEBUG_WATCHDOG ? reason_str[reason] : "<unknown>"); 64 #endif 65 // Dump a backtrace of the frame which caused the switch to secondary mode: 66 nentries = backtrace(bt, sizeof(bt) / sizeof(bt[0])); 67 backtrace_symbols_fd(bt, nentries, fileno(stdout)); 68 } 69 #else // SIGDEBUG 70 void warn_upon_switch(int sig __attribute__((unused))) { 71 void *bt[32]; 72 int nentries; 73 74 /* Dump a backtrace of the frame which caused the switch to 75 secondary mode: */ 76 nentries = backtrace(bt, sizeof(bt) / sizeof(bt[0])); 77 backtrace_symbols_fd(bt, nentries, fileno(stdout)); 78 } 79 #endif // SIGDEBUG 85 80 #endif //__XENO__ 86 void seg_fault(int sig __attribute__((unused))) { 87 void *bt[32]; 88 int nentries; 89 90 printf("Segmentation fault:\n"); 91 /* Dump a backtrace of the frame which caused the segfault: */ 92 nentries = backtrace(bt,sizeof(bt) / sizeof(bt[0])); 93 backtrace_symbols_fd(bt,nentries,fileno(stdout)); 94 95 exit(1); 96 } 97 } 98 99 FrameworkManager_impl::FrameworkManager_impl(FrameworkManager *self,string name): Thread(self,"FrameworkManager",FRAMEWORK_TASK_PRIORITY) { 100 this->self=self; 101 is_logging=false; 102 logger_defined=false; 103 disable_errors=false; 104 ui_defined=false; 105 rcv_buf=NULL; 106 _this=this; 107 108 // Avoids memory swapping for this program 109 mlockall(MCL_CURRENT|MCL_FUTURE); 110 struct sigaction sa; 111 112 //catch segfault 113 signal(SIGSEGV, seg_fault); 114 115 //catch primary->secondary switch 81 void seg_fault(int sig __attribute__((unused))) { 82 void *bt[32]; 83 int nentries; 84 85 printf("Segmentation fault:\n"); 86 /* Dump a backtrace of the frame which caused the segfault: */ 87 nentries = backtrace(bt, sizeof(bt) / sizeof(bt[0])); 88 backtrace_symbols_fd(bt, nentries, fileno(stdout)); 89 90 exit(1); 91 } 92 } 93 94 FrameworkManager_impl::FrameworkManager_impl(FrameworkManager *self, 95 string name) 96 : Thread(self, "FrameworkManager", FRAMEWORK_TASK_PRIORITY) { 97 this->self = self; 98 is_logging = false; 99 logger_defined = false; 100 disable_errors = false; 101 ui_defined = false; 102 rcv_buf = NULL; 103 _this = this; 104 105 // Avoids memory swapping for this program 106 mlockall(MCL_CURRENT | MCL_FUTURE); 107 struct sigaction sa; 108 109 // catch segfault 110 signal(SIGSEGV, seg_fault); 111 112 // catch primary->secondary switch 116 113 #ifdef __XENO__ 117 114 #ifdef SIGDEBUG 118 119 120 121 122 #else 123 124 #endif 125 126 string task_name="Framework_"+name;127 int status=rt_task_shadow(NULL,task_name.c_str(),10,0);128 if(status!=0) {129 self->Err("rt_task_shadow error (%s)\n",strerror(-status));130 115 sigemptyset(&sa.sa_mask); 116 sa.sa_sigaction = warn_upon_switch; 117 sa.sa_flags = SA_SIGINFO; 118 sigaction(SIGDEBUG, &sa, NULL); 119 #else 120 signal(SIGXCPU, warn_upon_switch); 121 #endif 122 123 string task_name = "Framework_" + name; 124 int status = rt_task_shadow(NULL, task_name.c_str(), 10, 0); 125 if (status != 0) { 126 self->Err("rt_task_shadow error (%s)\n", strerror(-status)); 127 } 131 128 132 129 #endif //__XENO__ … … 134 131 135 132 void FrameworkManager_impl::ConnectionLost(void) { 136 137 138 connection_lost=true;133 Err("connection lost\n"); 134 gcs_watchdog->SafeStop(); 135 connection_lost = true; 139 136 } 140 137 141 138 FrameworkManager_impl::~FrameworkManager_impl() { 142 //Printf("destruction FrameworkManager_impl\n"); 143 int status; 144 145 SafeStop(); 146 Join(); 147 148 if(rcv_buf!=NULL) free(rcv_buf); 149 150 if(logger_defined==true) { 151 continuer=false; 152 (void)pthread_join(log_th,NULL); 153 154 status=DeletePipe(&cmd_pipe); 155 if(status!=0) { 156 Err("Error deleting pipe (%s)\n",strerror(-status)); 139 // Printf("destruction FrameworkManager_impl\n"); 140 int status; 141 142 SafeStop(); 143 Join(); 144 145 if (rcv_buf != NULL) 146 free(rcv_buf); 147 148 if (logger_defined == true) { 149 continuer = false; 150 (void)pthread_join(log_th, NULL); 151 152 status = DeletePipe(&cmd_pipe); 153 if (status != 0) { 154 Err("Error deleting pipe (%s)\n", strerror(-status)); 155 } 156 status = DeletePipe(&data_pipe); 157 if (status != 0) { 158 Err("Error deleting pipe (%s)\n", strerror(-status)); 159 } 160 161 #ifdef __XENO__ 162 status = rt_heap_delete(&log_heap); 163 if (status != 0) { 164 Err("rt_heap_delete error (%s)\n", strerror(-status)); 165 } 166 #endif 167 168 logs.clear(); 169 } 170 171 if (file_doc != NULL) 172 xmlFreeDoc(file_doc); 173 174 if (ui_defined) 175 delete top_layout; 176 177 if (com != NULL) { 178 delete com; 179 status = UDT::close(com_sock); 180 if (status != 0) 181 printf("Error udt::close %s", UDT::getlasterror().getErrorMessage()); 182 183 status = UDT::close(file_sock); 184 if (status != 0) 185 printf("Error udt::close %s", UDT::getlasterror().getErrorMessage()); 186 187 SleepMS(200); // a revoir, sinon UDT::cleanup bloque en RT 188 UDT::cleanup(); 189 } 190 191 // Printf("destruction FrameworkManager_impl ok\n"); 192 } 193 194 void FrameworkManager_impl::SetupConnection(string address, uint16_t port, 195 size_t rcv_buf_size) { 196 UDT::startup(); 197 this->rcv_buf_size = rcv_buf_size; 198 199 // socket file_socket, doit ĂŞtre crĂ©Ă© en premier, cf station sol 200 Printf("Connecting to %s:%i\n", address.c_str(), port); 201 file_sock = GetSocket(address, port); 202 com_sock = GetSocket(address, port); 203 204 // receive buffer allocation 205 rcv_buf = (char *)malloc(rcv_buf_size); 206 if (rcv_buf == NULL) { 207 Err("receive buffer malloc error\n"); 208 } 209 210 com = new ui_com(this, com_sock); 211 212 // file managment 213 bool blocking = true; 214 UDT::setsockopt(file_sock, 0, UDT_SNDSYN, &blocking, sizeof(bool)); 215 UDT::setsockopt(file_sock, 0, UDT_RCVSYN, &blocking, sizeof(bool)); 216 217 // configure timeout of blocking receive, short timeout to have priority on 218 // the other socket (see run method) 219 int timeout = 1; // ms 220 UDT::setsockopt(file_sock, 0, UDT_RCVTIMEO, &timeout, sizeof(int)); 221 222 timeout = 100; // ms 223 UDT::setsockopt(com_sock, 0, UDT_RCVTIMEO, &timeout, sizeof(int)); 224 225 Start(); 226 227 // watchdog for connection with ground station 228 connection_lost = false; 229 gcs_watchdog = new Watchdog( 230 this, std::bind(&FrameworkManager_impl::ConnectionLost, this), 231 (Time)1000000000); 232 gcs_watchdog->Start(); 233 } 234 235 void FrameworkManager_impl::SetupUserInterface(string xml_file) { 236 ui_defined = true; 237 this->xml_file = xml_file; 238 239 // top_layout=new Layout(NULL,XML_ROOT_ELEMENT,XML_ROOT_TYPE); 240 top_layout = new Layout(NULL, self->ObjectName(), XML_ROOT_TYPE); 241 242 // xml setup of the main widget 243 if (xml_file != "") { 244 xmlNodePtr *file_node = &(((Widget *)(top_layout))->pimpl_->file_node); 245 file_doc = xmlParseFile(xml_file.c_str()); 246 if (file_doc == NULL) { 247 self->Warn("XML document not parsed successfully. Creating a new one.\n"); 248 file_doc = xmlNewDoc((xmlChar *)"1.0"); 249 *file_node = xmlNewNode(NULL, (xmlChar *)XML_ROOT_TYPE); 250 xmlSetProp(*file_node, (xmlChar *)"name", 251 (xmlChar *)ObjectName().c_str()); 252 xmlDocSetRootElement(file_doc, *file_node); 253 // PrintXml(); 254 } else { 255 *file_node = xmlDocGetRootElement(file_doc); 256 if (xmlStrcmp((*file_node)->name, (xmlChar *)XML_ROOT_TYPE)) { 257 self->Warn("%s, no match found in xml file\n", XML_ROOT_TYPE); 258 *file_node = xmlNewNode(NULL, (xmlChar *)XML_ROOT_TYPE); 259 xmlSetProp(*file_node, (xmlChar *)"name", 260 (xmlChar *)ObjectName().c_str()); 261 xmlDocSetRootElement(file_doc, *file_node); 262 } 263 } 264 } else { 265 self->Err("xml file not defined\n"); 266 } 267 268 // gui 269 tabwidget = 270 new TabWidget(top_layout->At(0, 0), XML_MAIN_TABWIDGET, TabWidget::North); 271 save_button = 272 new PushButton(top_layout->At(1, 0), 273 "save config on target (" + self->ObjectName() + ")"); 274 // load_button=new PushButton(top_layout->At(1,1),"load config on target (" + 275 // self->ObjectName() + ")"); 276 } 277 278 // in case of RT, this thread switches to secondary mode when calling 279 // com->Receive 280 // it switches back to RT in ProcessXML when mutex are locked 281 void FrameworkManager_impl::Run(void) { 282 while (!ToBeStopped()) { 283 ssize_t bytesRead; 284 285 bytesRead = com->Receive(rcv_buf, rcv_buf_size); 286 287 if (bytesRead == (ssize_t)rcv_buf_size) 288 Err("FrameworkManager max receive size, augmenter le buffer size!\n"); 289 290 if (bytesRead > 0) { 291 // printf("recu %ld, trame %x\n",bytesRead,(uint8_t)rcv_buf[0]); 292 // rcv_buf[bytesRead-1]=0;//pour affichage 293 // printf("%s\n",rcv_buf); 294 295 switch ((uint8_t)rcv_buf[0]) { 296 case XML_HEADER: { 297 xmlDoc *doc; 298 rcv_buf[bytesRead] = 0; 299 300 doc = xmlReadMemory(rcv_buf, (int)bytesRead, "include.xml", 301 "ISO-8859-1", 0); 302 xmlNode *cur_node = NULL; 303 304 for (cur_node = xmlDocGetRootElement(doc); cur_node; 305 cur_node = cur_node->next) { 306 if (cur_node->type == XML_ELEMENT_NODE) { 307 if (!xmlStrcmp(cur_node->name, (xmlChar *)XML_ROOT_TYPE)) { 308 #ifdef __XENO__ 309 WarnUponSwitches( 310 true); // ProcessXML should not switch to secondary mode 311 #endif 312 top_layout->Widget::pimpl_->ProcessXML(cur_node->children); 313 #ifdef __XENO__ 314 WarnUponSwitches(false); // other parts of this thread can switch 315 // to secondary mode 316 #endif 317 break; 318 } 319 } 157 320 } 158 status=DeletePipe(&data_pipe); 159 if(status!=0) { 160 Err("Error deleting pipe (%s)\n",strerror(-status)); 321 322 xmlFreeDoc(doc); 323 324 if (save_button->Clicked()) 325 SaveXml(); 326 327 SaveXmlChange(rcv_buf); 328 329 break; 330 } 331 case WATCHDOG_HEADER: { 332 gcs_watchdog->Touch(); 333 break; 334 } 335 default: 336 Err("unknown id: %x\n", (uint8_t)rcv_buf[0]); 337 break; 338 } 339 } else { 340 if (com->ConnectionLost()) 341 SleepMS(10); // avoid infinite loop in this case 342 } 343 } 344 } 345 346 void FrameworkManager_impl::SaveXml(void) { 347 if (ui_defined) 348 xmlSaveFormatFile(xml_file.c_str(), file_doc, 1); 349 } 350 351 void FrameworkManager_impl::SaveXmlChange(char *buf) { 352 if (is_logging == true) { 353 FILE *xml_change; 354 char filename[256]; 355 Time time = GetTime(); 356 357 sprintf(filename, "%s/changes_at_%lld.xml", log_path.c_str(), time); 358 359 xml_change = fopen(filename, "a"); 360 fprintf(xml_change, "%s", buf); 361 fclose(xml_change); 362 363 sprintf(filename, "changes_at_%lld.xml", time); 364 xml_changes.push_back(filename); 365 } 366 } 367 368 void FrameworkManager_impl::SendFile(string path, string name) { 369 char *buf, *more_buf; 370 int size; 371 filebuf *pbuf; 372 ssize_t nb_write; 373 string filename = path + "/" + name; 374 375 // open the file 376 fstream ifs(filename.c_str(), ios::in | ios::binary); 377 ifs.seekg(0, ios::end); 378 size = ifs.tellg(); 379 ifs.seekg(0, ios::beg); 380 pbuf = ifs.rdbuf(); 381 382 if (size <= 0) { 383 Err("error opening file %s\n", filename.c_str()); 384 return; 385 } 386 387 buf = (char *)malloc(sizeof(uint8_t) + sizeof(int) + name.size()); 388 if (buf == NULL) { 389 Err("malloc error, not sending file\n"); 390 return; 391 } 392 393 if (IsBigEndian()) { 394 buf[0] = FILE_INFO_BIG_ENDIAN; 395 } else { 396 buf[0] = FILE_INFO_LITTLE_ENDIAN; 397 } 398 399 memcpy(buf + 1, &size, sizeof(int)); 400 memcpy(buf + 1 + sizeof(int), name.c_str(), name.size()); 401 Printf("sending %s, size: %i\n", filename.c_str(), size); 402 // send file information 403 UDT::sendmsg(file_sock, buf, sizeof(uint8_t) + sizeof(int) + name.size(), -1, 404 true); 405 406 more_buf = (char *)realloc((void *)buf, size); 407 if (more_buf == NULL) { 408 Err("realloc error, not sending file\n"); 409 free(buf); 410 return; 411 } else { 412 buf = more_buf; 413 } 414 415 pbuf->sgetn(buf, size); 416 // send the file 417 nb_write = UDT::sendmsg(file_sock, buf, size, -1, true); 418 419 if (nb_write < 0) { 420 Err("UDT::sendmsg error (%s)\n", UDT::getlasterror().getErrorMessage()); 421 } else if (nb_write != size) { 422 Err("UDT::sendmsg error, sent %ld/%i\n", nb_write, size); 423 } 424 425 ifs.close(); 426 free(buf); 427 } 428 429 void FrameworkManager_impl::FinishSending() { 430 char rm_cmd[256]; 431 432 // send orignal xml 433 SendFile(log_path, "setup.xml"); 434 sprintf(rm_cmd, "rm %s/setup.xml", log_path.c_str()); 435 system(rm_cmd); 436 437 // send xml changes 438 for (size_t i = 0; i < xml_changes.size(); i++) { 439 // Printf("%s\n",xml_changes.at(i).c_str()); 440 SendFile(log_path, xml_changes.at(i).c_str()); 441 sprintf(rm_cmd, "rm %s/%s", log_path.c_str(), xml_changes.at(i).c_str()); 442 system(rm_cmd); 443 } 444 xml_changes.clear(); 445 446 // end notify 447 char buf = END; 448 int nb_write = UDT::sendmsg(file_sock, &buf, 1, -1, true); 449 450 if (nb_write < 0) { 451 Err("UDT::sendmsg error (%s)\n", UDT::getlasterror().getErrorMessage()); 452 } else if (nb_write != 1) { 453 Err("UDT::sendmsg error, sent %i/%i\n", nb_write, 1); 454 } 455 } 456 457 UDTSOCKET FrameworkManager_impl::GetSocket(string address, uint16_t port) { 458 while (1) { 459 UDTSOCKET new_fd; 460 new_fd = UDT::socket(AF_INET, SOCK_DGRAM, 0); 461 if (new_fd == UDT::INVALID_SOCK) { 462 Err("socket error: %s\n", UDT::getlasterror().getErrorMessage()); 463 return 0; 464 } 465 466 sockaddr_in serv_addr; 467 serv_addr.sin_family = AF_INET; 468 serv_addr.sin_port = htons(short(port)); 469 470 if (inet_pton(AF_INET, address.c_str(), &serv_addr.sin_addr) <= 0) { 471 Err("incorrect network address\n"); 472 return 0; 473 } 474 475 memset(&(serv_addr.sin_zero), '\0', 8); 476 477 if (UDT::ERROR == 478 UDT::connect(new_fd, (sockaddr *)&serv_addr, sizeof(serv_addr))) { 479 // Printf("connect error: %s 480 // %i\n",UDT::getlasterror().getErrorMessage(),UDT::getlasterror().getErrorCode()); 481 UDT::close(new_fd); 482 if (UDT::getlasterror().getErrorCode() != 1001 && 483 UDT::getlasterror().getErrorCode() != 1002) { 484 Err("connect error: %s\n", UDT::getlasterror().getErrorMessage()); 485 return 0; 486 } 487 } else { 488 // printf("connected to 489 // %s:%i\n",inet_ntoa(serv_addr.sin_addr),serv_addr.sin_port); 490 return new_fd; 491 } 492 } 493 } 494 495 #ifdef __XENO__ 496 int FrameworkManager_impl::CreatePipe(RT_PIPE *fd, string name) { 497 name = self->ObjectName() + "-" + name; 498 // xenomai limitation 499 if (name.size() > 31) 500 self->Err("rt_pipe_create error (%s is too long)\n", name.c_str()); 501 // start log writter 502 #ifdef RT_PIPE_SIZE 503 return rt_pipe_create(fd, name.c_str(), P_MINOR_AUTO, RT_PIPE_SIZE); 504 #else 505 return rt_pipe_create(fd, name.c_str(), P_MINOR_AUTO, 0); 506 #endif 507 } 508 #else 509 int FrameworkManager_impl::CreatePipe(int (*fd)[2], string name) { 510 // if(pipe2(fd[0],O_NONBLOCK) == -1) 511 if (pipe(fd[0]) == -1) { 512 return errno; 513 } else { 514 int attr = fcntl((*fd)[0], F_GETFL, 0); 515 if (attr == -1) { 516 return errno; 517 } 518 if (fcntl((*fd)[0], F_SETFL, attr | O_NONBLOCK) == -1) { 519 return errno; 520 } 521 attr = fcntl((*fd)[1], F_GETFL, 0); 522 if (attr == -1) { 523 return errno; 524 } 525 if (fcntl((*fd)[1], F_SETFL, attr | O_NONBLOCK) == -1) { 526 return errno; 527 } 528 529 return 0; 530 } 531 } 532 #endif 533 534 #ifdef __XENO__ 535 int FrameworkManager_impl::DeletePipe(RT_PIPE *fd) { 536 return rt_pipe_delete(fd); 537 } 538 #else 539 int FrameworkManager_impl::DeletePipe(int (*fd)[2]) { 540 int status1 = close((*fd)[0]); 541 int status2 = close((*fd)[1]); 542 if (status1 == 0 && status2 == 0) 543 return 0; 544 if (status1 != 0) 545 return status1; 546 if (status2 != 0) 547 return status2; 548 } 549 #endif 550 551 void FrameworkManager_impl::SetupLogger(string log_path) { 552 if (logger_defined == true) { 553 Warn("SetupLogger() was already called.\n"); 554 return; 555 } 556 557 this->log_path = log_path; 558 559 int status = CreatePipe(&cmd_pipe, "log_cmd"); 560 if (status != 0) { 561 Err("Error creating pipe (%s)\n", strerror(-status)); 562 return; 563 } 564 565 status = CreatePipe(&data_pipe, "log_data"); 566 if (status != 0) { 567 Err("Error creating pipe (%s)\n", strerror(-status)); 568 return; 569 } 570 571 #ifdef __XENO__ 572 string tmp_name; 573 tmp_name = self->ObjectName() + "-log_heap"; 574 status = rt_heap_create(&log_heap, tmp_name.c_str(), LOG_HEAP, H_FIFO); 575 if (status != 0) { 576 Err("rt_heap_create error (%s)\n", strerror(-status)); 577 return; 578 } 579 #endif //__XENO__ 580 581 continuer = true; 582 583 #ifdef NRT_STACK_SIZE 584 // Initialize thread creation attributes 585 pthread_attr_t attr; 586 if (pthread_attr_init(&attr) != 0) { 587 Err("pthread_attr_init error\n"); 588 return; 589 } 590 591 if (pthread_attr_setstacksize(&attr, NRT_STACK_SIZE) != 0) { 592 Err("pthread_attr_setstacksize error\n"); 593 return; 594 } 595 596 if (pthread_create(&log_th, &attr, write_log_user, (void *)this) < 0) 597 #else 598 if (pthread_create(&log_th, NULL, write_log_user, (void *)this) < 0) 599 #endif 600 { 601 Err("pthread_create error\n"); 602 return; 603 } 604 #ifdef NRT_STACK_SIZE 605 if (pthread_attr_destroy(&attr) != 0) { 606 Err("pthread_attr_destroy error\n"); 607 return; 608 } 609 #endif 610 611 logger_defined = true; 612 } 613 614 void FrameworkManager_impl::AddDeviceToLog(IODevice *device) { 615 if (logger_defined == false) { 616 Err("SetupLogger() was not called, not starting log\n"); 617 return; 618 } 619 620 if (is_logging == false) { 621 if (device->pimpl_->SetToBeLogged()) { 622 log_desc_t tmp; 623 tmp.device = device; 624 logs.push_back(tmp); 625 } else { 626 Warn("not adding it twice\n"); 627 } 628 } else { 629 Err("impossible while logging\n"); 630 } 631 } 632 633 void FrameworkManager_impl::StartLog(void) { 634 if (logger_defined == false) { 635 Err("SetupLogger() was not called, not starting log\n"); 636 return; 637 } 638 639 ssize_t written; 640 size_t nb_err = 0; 641 642 if (logs.size() == 0) { 643 Warn("Not starting log: nothing to log!\n"); 644 return; 645 } 646 647 if (is_logging == false) { 648 for (size_t i = 0; i < logs.size(); i++) { 649 650 logs.at(i).running = true; 651 logs.at(i).dbtFile = NULL; 652 logs.at(i).size = logs.at(i).device->pimpl_->LogSize(); 653 #ifdef __XENO__ 654 written = 655 rt_pipe_write(&cmd_pipe, &logs.at(i), sizeof(log_desc_t), P_NORMAL); 656 #else 657 written = write(cmd_pipe[1], &logs.at(i), sizeof(log_desc_t)); 658 #endif 659 660 if (written < 0) { 661 Err("write pipe error (%s)\n", strerror(-written)); 662 nb_err++; 663 logs.at(i).running = false; 664 } else if (written != sizeof(log_desc_t)) { 665 Err("write pipe error %ld/%ld\n", written, sizeof(log_desc_t)); 666 nb_err++; 667 logs.at(i).running = false; 668 } 669 } 670 671 if (nb_err != logs.size()) 672 is_logging = true; 673 } else { 674 Warn("Already logging\n"); 675 } 676 } 677 678 void FrameworkManager_impl::StopLog(void) { 679 ssize_t written; 680 681 if (is_logging == true) { 682 for (size_t i = 0; i < logs.size(); i++) { 683 logs.at(i).running = false; 684 } 685 // send only one running false condition, user thread will stop and send all 686 #ifdef __XENO__ 687 written = 688 rt_pipe_write(&cmd_pipe, &logs.at(0), sizeof(log_desc_t), P_NORMAL); 689 #else 690 written = write(cmd_pipe[1], &logs.at(0), sizeof(log_desc_t)); 691 #endif 692 693 if (written < 0) { 694 Err("write pipe error (%s)\n", strerror(-written)); 695 return; 696 } else if (written != sizeof(log_desc_t)) { 697 Err("write pipe error %ld/%ld\n", written, sizeof(log_desc_t)); 698 return; 699 } 700 701 is_logging = false; 702 } else { 703 Warn("Not logging\n"); 704 } 705 } 706 707 char *FrameworkManager_impl::GetBuffer(size_t sz) { 708 // Printf("alloc %i\n",sz); 709 #ifdef __XENO__ 710 void *ptr; 711 int status = rt_heap_alloc(&log_heap, sz, TM_NONBLOCK, &ptr); 712 if (status != 0) { 713 Err("rt_heap_alloc error (%s)\n", strerror(-status)); 714 ptr = NULL; 715 } 716 return (char *)ptr; 717 #else 718 return (char *)malloc(sz); 719 #endif 720 } 721 722 void FrameworkManager_impl::ReleaseBuffer(char *buf) { 723 #ifdef __XENO__ 724 int status = rt_heap_free(&log_heap, buf); 725 726 if (status != 0) { 727 Err("rt_heap_free error (%s)\n", strerror(-status)); 728 } 729 #else 730 free(buf); 731 #endif 732 } 733 734 void FrameworkManager_impl::WriteLog(const char *buf, size_t size) { 735 ssize_t written; 736 737 #ifdef __XENO__ 738 written = rt_pipe_write(&data_pipe, buf, size, P_NORMAL); 739 #else 740 written = write(data_pipe[1], buf, size); 741 #endif 742 743 if (written < 0) { 744 Err("erreur write pipe (%s)\n", strerror(-written)); 745 } else if (written != (ssize_t)size) { 746 Err("erreur write pipe %ld/%ld\n", written, size); 747 } 748 } 749 750 void *FrameworkManager_impl::write_log_user(void *arg) { 751 int cmd_pipe = -1; 752 int data_pipe = -1; 753 string filename; 754 fd_set set; 755 struct timeval timeout; 756 FrameworkManager_impl *caller = (FrameworkManager_impl *)arg; 757 int rv; 758 759 vector<log_desc_t> logs; 760 761 #ifdef __XENO__ 762 filename = NRT_PIPE_PATH + caller->self->ObjectName() + "-log_cmd"; 763 while (cmd_pipe < 0) { 764 cmd_pipe = open(filename.c_str(), O_RDWR); 765 if (cmd_pipe < 0 && errno != ENOENT) 766 caller->self->Err("open rt_pipe error: %s %i\n", filename.c_str(), errno); 767 usleep(1000); 768 } 769 filename = NRT_PIPE_PATH + caller->self->ObjectName() + "-log_data"; 770 while (data_pipe < 0) { 771 data_pipe = open(filename.c_str(), O_RDWR); 772 if (data_pipe < 0 && errno != ENOENT) 773 caller->self->Err("open rt_pipe error: %s %i\n", filename.c_str(), errno); 774 usleep(1000); 775 } 776 #else 777 cmd_pipe = caller->cmd_pipe[0]; 778 data_pipe = caller->data_pipe[0]; 779 #endif 780 781 while (caller->continuer == true) { 782 FD_ZERO(&set); 783 FD_SET(cmd_pipe, &set); 784 FD_SET(data_pipe, &set); 785 786 timeout.tv_sec = 0; 787 timeout.tv_usec = SELECT_TIMEOUT_MS * 1000; 788 rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout); 789 790 if (rv == -1) { 791 caller->Err("select error\n"); // an error accured 792 } else if (rv == 0) { 793 // printf("timeout write_log_user %s\n",caller->ObjectName().c_str()); // 794 // a timeout occured 795 } else { 796 if (FD_ISSET(cmd_pipe, &set)) { 797 log_desc_t tmp; 798 read(cmd_pipe, &tmp, sizeof(log_desc_t)); 799 800 if (tmp.running == true) // start logging 801 { 802 filename = 803 caller->log_path + "/" + caller->FileName(tmp.device) + ".dbt"; 804 printf("Creating log file %s (log size %i)\n", filename.c_str(), 805 (int)tmp.size); 806 tmp.dbtFile = inithdFile((char *)filename.c_str(), UAV, tmp.size); 807 logs.push_back(tmp); 808 809 if (logs.size() == 1) { 810 filename = caller->log_path + "/setup.xml"; 811 xmlSaveFile(filename.c_str(), caller->file_doc); 812 } 813 } else // stop logging 814 { 815 for (size_t i = 0; i < logs.size(); i++) { 816 if (logs.at(i).dbtFile != NULL) { 817 close_hdfile(logs.at(i).dbtFile); 818 819 filename = caller->FileName(logs.at(i).device) + ".dbt"; 820 caller->SendFile(caller->log_path, filename); 821 822 fstream txt_file; 823 filename = caller->FileName(logs.at(i).device) + ".txt"; 824 txt_file.open((caller->log_path + "/" + filename).c_str(), 825 fstream::out); 826 txt_file << "1: time (us)\n2: time (ns)\n"; 827 int index = 3; 828 logs.at(i).device->pimpl_->WriteLogsDescriptors(txt_file, &index); 829 txt_file.close(); 830 831 caller->SendFile(caller->log_path, filename); 832 } 833 } 834 // a revoir celui ci est le xml enregistrĂ© et pas forcement l'actuel 835 // if(caller->xml_file!="") caller->SendFile(caller->xml_file); 836 caller->FinishSending(); 837 838 logs.clear(); 161 839 } 162 163 #ifdef __XENO__ 164 status=rt_heap_delete(&log_heap); 165 if(status!=0) { 166 Err("rt_heap_delete error (%s)\n",strerror(-status)); 840 } 841 842 if (FD_ISSET(data_pipe, &set)) { 843 log_header_t header; 844 read(data_pipe, &header, sizeof(log_header_t)); 845 846 for (size_t i = 0; i < logs.size(); i++) { 847 if (logs.at(i).device == header.device) { 848 char *buf = (char *)malloc(header.size); 849 read(data_pipe, buf, header.size); 850 // printf("%s \n",header.device->ObjectName().c_str()); 851 // for(int i=0;i<header.size;i++) printf("%x ",buf[i]); 852 // printf("\n"); 853 if (logs.at(i).size == header.size) { 854 if (logs.at(i).dbtFile != NULL) { 855 write_hdfile( 856 logs.at(i).dbtFile, buf, (road_time_t)(header.time / 1000), 857 (road_timerange_t)(header.time % 1000), header.size); 858 } else { 859 printf("err\n"); 860 } 861 } else { 862 caller->self->Err("%s log size is not correct %i/%i\n", 863 header.device->ObjectName().c_str(), 864 header.size, logs.at(i).size); 865 } 866 free(buf); 867 } 167 868 } 168 #endif 169 170 logs.clear(); 171 } 172 173 if(file_doc!=NULL) xmlFreeDoc(file_doc); 174 175 if(ui_defined) delete top_layout; 176 177 if(com!=NULL) { 178 delete com; 179 status=UDT::close(com_sock); 180 if(status!=0) printf("Error udt::close %s",UDT::getlasterror().getErrorMessage()); 181 182 status=UDT::close(file_sock); 183 if(status!=0) printf("Error udt::close %s",UDT::getlasterror().getErrorMessage()); 184 185 SleepMS(200);//a revoir, sinon UDT::cleanup bloque en RT 186 UDT::cleanup(); 187 } 188 189 //Printf("destruction FrameworkManager_impl ok\n"); 190 } 191 192 193 void FrameworkManager_impl::SetupConnection(string address,uint16_t port,size_t rcv_buf_size) { 194 UDT::startup(); 195 this->rcv_buf_size=rcv_buf_size; 196 197 //socket file_socket, doit ĂŞtre crĂ©Ă© en premier, cf station sol 198 Printf("Connecting to %s:%i\n",address.c_str(),port); 199 file_sock=GetSocket(address,port); 200 com_sock=GetSocket(address,port); 201 202 //receive buffer allocation 203 rcv_buf=(char*)malloc(rcv_buf_size); 204 if(rcv_buf==NULL) { 205 Err("receive buffer malloc error\n"); 206 } 207 208 com=new ui_com(this,com_sock); 209 210 //file managment 211 bool blocking = true; 212 UDT::setsockopt(file_sock, 0, UDT_SNDSYN, &blocking, sizeof(bool)); 213 UDT::setsockopt(file_sock, 0, UDT_RCVSYN, &blocking, sizeof(bool)); 214 215 //configure timeout of blocking receive, short timeout to have priority on the other socket (see run method) 216 int timeout=1;//ms 217 UDT::setsockopt(file_sock, 0, UDT_RCVTIMEO, &timeout, sizeof(int)); 218 219 timeout=100;//ms 220 UDT::setsockopt(com_sock, 0, UDT_RCVTIMEO, &timeout, sizeof(int)); 221 222 Start(); 223 224 //watchdog for connection with ground station 225 connection_lost=false; 226 gcs_watchdog=new Watchdog(this,std::bind(&FrameworkManager_impl::ConnectionLost,this),(Time)1000000000); 227 gcs_watchdog->Start(); 228 } 229 230 void FrameworkManager_impl::SetupUserInterface(string xml_file) { 231 ui_defined=true; 232 this->xml_file=xml_file; 233 234 //top_layout=new Layout(NULL,XML_ROOT_ELEMENT,XML_ROOT_TYPE); 235 top_layout=new Layout(NULL,self->ObjectName(),XML_ROOT_TYPE); 236 237 //xml setup of the main widget 238 if(xml_file!="") { 239 xmlNodePtr* file_node=&(((Widget*)(top_layout))->pimpl_->file_node); 240 file_doc = xmlParseFile(xml_file.c_str()); 241 if (file_doc == NULL ) { 242 self->Warn("XML document not parsed successfully. Creating a new one.\n"); 243 file_doc = xmlNewDoc((xmlChar*)"1.0"); 244 *file_node=xmlNewNode(NULL, (xmlChar*)XML_ROOT_TYPE); 245 xmlSetProp(*file_node,(xmlChar*)"name",(xmlChar*)ObjectName().c_str()); 246 xmlDocSetRootElement(file_doc, *file_node); 247 //PrintXml(); 248 } else { 249 *file_node=xmlDocGetRootElement(file_doc); 250 if(xmlStrcmp((*file_node)->name,(xmlChar*)XML_ROOT_TYPE)) { 251 self->Warn("%s, no match found in xml file\n",XML_ROOT_TYPE); 252 *file_node=xmlNewNode(NULL, (xmlChar*)XML_ROOT_TYPE); 253 xmlSetProp(*file_node,(xmlChar*)"name",(xmlChar*)ObjectName().c_str()); 254 xmlDocSetRootElement(file_doc, *file_node); 255 } 256 } 257 } else { 258 self->Err("xml file not defined\n"); 259 } 260 261 //gui 262 tabwidget=new TabWidget(top_layout->At(0,0),XML_MAIN_TABWIDGET,TabWidget::North); 263 save_button=new PushButton(top_layout->At(1,0),"save config on target (" + self->ObjectName() + ")"); 264 //load_button=new PushButton(top_layout->At(1,1),"load config on target (" + self->ObjectName() + ")"); 265 } 266 267 //in case of RT, this thread switches to secondary mode when calling com->Receive 268 //it switches back to RT in ProcessXML when mutex are locked 269 void FrameworkManager_impl::Run(void) 270 { 271 while(!ToBeStopped()) 272 { 273 ssize_t bytesRead; 274 275 bytesRead=com->Receive(rcv_buf,rcv_buf_size); 276 277 if(bytesRead==(ssize_t)rcv_buf_size) Err("FrameworkManager max receive size, augmenter le buffer size!\n"); 278 279 if(bytesRead>0) 280 { 281 //printf("recu %ld, trame %x\n",bytesRead,(uint8_t)rcv_buf[0]); 282 //rcv_buf[bytesRead-1]=0;//pour affichage 283 //printf("%s\n",rcv_buf); 284 285 switch((uint8_t)rcv_buf[0]) 286 { 287 case XML_HEADER: 288 { 289 xmlDoc *doc; 290 rcv_buf[bytesRead]=0; 291 292 doc = xmlReadMemory(rcv_buf, (int)bytesRead, "include.xml", "ISO-8859-1", 0); 293 xmlNode *cur_node = NULL; 294 295 for (cur_node = xmlDocGetRootElement(doc); cur_node; cur_node = cur_node->next) 296 { 297 if (cur_node->type == XML_ELEMENT_NODE) 298 { 299 if(!xmlStrcmp(cur_node->name,(xmlChar*)XML_ROOT_TYPE) ) 300 { 301 #ifdef __XENO__ 302 WarnUponSwitches(true);//ProcessXML should not switch to secondary mode 303 #endif 304 top_layout->Widget::pimpl_->ProcessXML(cur_node->children); 305 #ifdef __XENO__ 306 WarnUponSwitches(false);//other parts of this thread can switch to secondary mode 307 #endif 308 break; 309 } 310 } 311 } 312 313 xmlFreeDoc(doc); 314 315 if(save_button->Clicked()) SaveXml(); 316 317 SaveXmlChange(rcv_buf); 318 319 break; 320 } 321 case WATCHDOG_HEADER: { 322 gcs_watchdog->Touch(); 323 break; 324 } 325 default: 326 Err("unknown id: %x\n",(uint8_t)rcv_buf[0]); 327 break; 328 } 329 } else { 330 if(com->ConnectionLost()) SleepMS(10);//avoid infinite loop in this case 331 } 332 } 333 } 334 335 void FrameworkManager_impl::SaveXml(void) 336 { 337 if(ui_defined) xmlSaveFormatFile(xml_file.c_str(),file_doc,1); 338 } 339 340 void FrameworkManager_impl::SaveXmlChange(char* buf) 341 { 342 if(is_logging==true) 343 { 344 FILE *xml_change; 345 char filename[256]; 346 Time time=GetTime(); 347 348 sprintf(filename,"%s/changes_at_%lld.xml",log_path.c_str(),time); 349 350 xml_change=fopen(filename,"a"); 351 fprintf(xml_change,"%s",buf); 352 fclose(xml_change); 353 354 sprintf(filename,"changes_at_%lld.xml",time); 355 xml_changes.push_back(filename); 356 } 357 } 358 359 void FrameworkManager_impl::SendFile(string path,string name) 360 { 361 char *buf,*more_buf; 362 int size; 363 filebuf *pbuf; 364 ssize_t nb_write; 365 string filename=path+ "/" +name; 366 367 // open the file 368 fstream ifs(filename.c_str(), ios::in | ios::binary); 369 ifs.seekg(0, ios::end); 370 size = ifs.tellg(); 371 ifs.seekg(0, ios::beg); 372 pbuf=ifs.rdbuf(); 373 374 if(size<=0) 375 { 376 Err("error opening file %s\n",filename.c_str()); 377 return; 378 } 379 380 buf=(char*)malloc(sizeof(uint8_t)+sizeof(int)+name.size()); 381 if(buf==NULL) 382 { 383 Err("malloc error, not sending file\n"); 384 return; 385 } 386 387 if(IsBigEndian()) 388 { 389 buf[0]=FILE_INFO_BIG_ENDIAN; 390 } 391 else 392 { 393 buf[0]=FILE_INFO_LITTLE_ENDIAN; 394 } 395 396 memcpy(buf+1,&size,sizeof(int)); 397 memcpy(buf+1+sizeof(int),name.c_str(),name.size()); 398 Printf("sending %s, size: %i\n",filename.c_str(),size); 399 // send file information 400 UDT::sendmsg(file_sock, buf, sizeof(uint8_t)+sizeof(int)+name.size(), -1,true); 401 402 more_buf=(char*)realloc((void*)buf,size); 403 if(more_buf==NULL) 404 { 405 Err("realloc error, not sending file\n"); 406 free(buf); 407 return; 408 } 409 else 410 { 411 buf=more_buf; 412 } 413 414 pbuf->sgetn (buf,size); 415 // send the file 416 nb_write=UDT::sendmsg(file_sock, buf, size,-1,true); 417 418 if(nb_write<0) 419 { 420 Err("UDT::sendmsg error (%s)\n",UDT::getlasterror().getErrorMessage()); 421 } 422 else if (nb_write != size) 423 { 424 Err("UDT::sendmsg error, sent %ld/%i\n",nb_write,size); 425 } 426 427 ifs.close(); 428 free(buf); 429 } 430 431 void FrameworkManager_impl::FinishSending() 432 { 433 char rm_cmd[256]; 434 435 //send orignal xml 436 SendFile(log_path,"setup.xml"); 437 sprintf(rm_cmd,"rm %s/setup.xml",log_path.c_str()); 438 system(rm_cmd); 439 440 //send xml changes 441 for(size_t i=0;i<xml_changes.size();i++) 442 { 443 //Printf("%s\n",xml_changes.at(i).c_str()); 444 SendFile(log_path,xml_changes.at(i).c_str()); 445 sprintf(rm_cmd,"rm %s/%s",log_path.c_str(),xml_changes.at(i).c_str()); 446 system(rm_cmd); 447 } 448 xml_changes.clear(); 449 450 //end notify 451 char buf=END; 452 int nb_write=UDT::sendmsg(file_sock, &buf, 1,-1,true); 453 454 if(nb_write<0) 455 { 456 Err("UDT::sendmsg error (%s)\n",UDT::getlasterror().getErrorMessage()); 457 } 458 else if (nb_write != 1) 459 { 460 Err("UDT::sendmsg error, sent %i/%i\n",nb_write,1); 461 } 462 } 463 464 UDTSOCKET FrameworkManager_impl::GetSocket(string address,uint16_t port) { 465 while(1) { 466 UDTSOCKET new_fd; 467 new_fd = UDT::socket(AF_INET, SOCK_DGRAM, 0); 468 if(new_fd==UDT::INVALID_SOCK) { 469 Err("socket error: %s\n",UDT::getlasterror().getErrorMessage()); 470 return 0; 471 } 472 473 sockaddr_in serv_addr; 474 serv_addr.sin_family = AF_INET; 475 serv_addr.sin_port = htons(short(port)); 476 477 if (inet_pton(AF_INET,address.c_str(), &serv_addr.sin_addr) <= 0) { 478 Err("incorrect network address\n"); 479 return 0; 480 } 481 482 memset(&(serv_addr.sin_zero), '\0', 8); 483 484 if (UDT::ERROR == UDT::connect(new_fd, (sockaddr*)&serv_addr, sizeof(serv_addr))) { 485 //Printf("connect error: %s %i\n",UDT::getlasterror().getErrorMessage(),UDT::getlasterror().getErrorCode()); 486 UDT::close(new_fd); 487 if(UDT::getlasterror().getErrorCode()!=1001 && UDT::getlasterror().getErrorCode()!=1002) { 488 Err("connect error: %s\n",UDT::getlasterror().getErrorMessage()); 489 return 0; 490 } 491 } else { 492 //printf("connected to %s:%i\n",inet_ntoa(serv_addr.sin_addr),serv_addr.sin_port); 493 return new_fd; 494 } 495 } 496 } 497 498 #ifdef __XENO__ 499 int FrameworkManager_impl::CreatePipe(RT_PIPE* fd,string name) 500 { 501 name=self->ObjectName()+ "-" + name; 502 //xenomai limitation 503 if(name.size()>31) self->Err("rt_pipe_create error (%s is too long)\n",name.c_str()); 504 //start log writter 505 #ifdef RT_PIPE_SIZE 506 return rt_pipe_create(fd, name.c_str(), P_MINOR_AUTO,RT_PIPE_SIZE); 507 #else 508 return rt_pipe_create(fd, name.c_str(), P_MINOR_AUTO,0); 509 #endif 510 } 511 #else 512 int FrameworkManager_impl::CreatePipe(int (*fd)[2],string name) 513 { 514 //if(pipe2(fd[0],O_NONBLOCK) == -1) 515 if(pipe(fd[0]) == -1) 516 { 517 return errno; 518 } 519 else 520 { 521 int attr=fcntl((*fd)[0], F_GETFL,0); 522 if(attr == -1) 523 { 524 return errno; 525 } 526 if(fcntl((*fd)[0], F_SETFL, attr|O_NONBLOCK)== -1) 527 { 528 return errno; 529 } 530 attr=fcntl((*fd)[1], F_GETFL,0); 531 if(attr == -1) 532 { 533 return errno; 534 } 535 if(fcntl((*fd)[1], F_SETFL, attr|O_NONBLOCK)== -1) 536 { 537 return errno; 538 } 539 540 return 0; 541 } 542 } 543 #endif 544 545 #ifdef __XENO__ 546 int FrameworkManager_impl::DeletePipe(RT_PIPE* fd) 547 { 548 return rt_pipe_delete(fd); 549 } 550 #else 551 int FrameworkManager_impl::DeletePipe(int (*fd)[2]) 552 { 553 int status1=close((*fd)[0]); 554 int status2=close((*fd)[1]); 555 if(status1==0 && status2==0) return 0; 556 if(status1!=0) return status1; 557 if(status2!=0) return status2; 558 } 559 #endif 560 561 void FrameworkManager_impl::SetupLogger(string log_path) { 562 if(logger_defined==true) { 563 Warn("SetupLogger() was already called.\n"); 564 return; 565 } 566 567 this->log_path=log_path; 568 569 int status=CreatePipe(&cmd_pipe,"log_cmd"); 570 if(status!=0) { 571 Err("Error creating pipe (%s)\n",strerror(-status)); 572 return; 573 } 574 575 status=CreatePipe(&data_pipe,"log_data"); 576 if(status!=0) { 577 Err("Error creating pipe (%s)\n",strerror(-status)); 578 return; 579 } 580 581 #ifdef __XENO__ 582 string tmp_name; 583 tmp_name=self->ObjectName() + "-log_heap"; 584 status=rt_heap_create(&log_heap,tmp_name.c_str(),LOG_HEAP,H_FIFO); 585 if(status!=0) { 586 Err("rt_heap_create error (%s)\n",strerror(-status)); 587 return; 588 } 589 #endif //__XENO__ 590 591 continuer=true; 592 593 #ifdef NRT_STACK_SIZE 594 // Initialize thread creation attributes 595 pthread_attr_t attr; 596 if(pthread_attr_init(&attr) != 0) { 597 Err("pthread_attr_init error\n"); 598 return; 599 } 600 601 if(pthread_attr_setstacksize(&attr, NRT_STACK_SIZE) != 0) { 602 Err("pthread_attr_setstacksize error\n"); 603 return; 604 } 605 606 if(pthread_create(&log_th, &attr, write_log_user, (void*)this) < 0) 607 #else 608 if(pthread_create(&log_th, NULL, write_log_user, (void*)this) < 0) 609 #endif 610 { 611 Err("pthread_create error\n"); 612 return; 613 } 614 #ifdef NRT_STACK_SIZE 615 if(pthread_attr_destroy(&attr) != 0) { 616 Err("pthread_attr_destroy error\n"); 617 return; 618 } 619 #endif 620 621 logger_defined=true; 622 } 623 624 void FrameworkManager_impl::AddDeviceToLog(IODevice *device) 625 { 626 if(logger_defined==false) 627 { 628 Err("SetupLogger() was not called, not starting log\n"); 629 return; 630 } 631 632 if(is_logging==false) 633 { 634 if(device->pimpl_->SetToBeLogged()) { 635 log_desc_t tmp; 636 tmp.device=device; 637 logs.push_back(tmp); 638 } else { 639 Warn("not adding it twice\n"); 640 } 641 } 642 else 643 { 644 Err("impossible while logging\n"); 645 } 646 } 647 648 void FrameworkManager_impl::StartLog(void) 649 { 650 if(logger_defined==false) 651 { 652 Err("SetupLogger() was not called, not starting log\n"); 653 return; 654 } 655 656 ssize_t written; 657 size_t nb_err=0; 658 659 if(logs.size()==0) 660 { 661 Warn("Not starting log: nothing to log!\n"); 662 return; 663 } 664 665 if(is_logging==false) 666 { 667 for(size_t i=0;i<logs.size();i++) 668 { 669 670 logs.at(i).running=true; 671 logs.at(i).dbtFile=NULL; 672 logs.at(i).size=logs.at(i).device->pimpl_->LogSize(); 673 #ifdef __XENO__ 674 written=rt_pipe_write(&cmd_pipe,&logs.at(i),sizeof(log_desc_t),P_NORMAL); 675 #else 676 written=write(cmd_pipe[1],&logs.at(i),sizeof(log_desc_t)); 677 #endif 678 679 if(written<0) 680 { 681 Err("write pipe error (%s)\n",strerror(-written)); 682 nb_err++; 683 logs.at(i).running=false; 684 } 685 else if (written != sizeof(log_desc_t)) 686 { 687 Err("write pipe error %ld/%ld\n",written,sizeof(log_desc_t)); 688 nb_err++; 689 logs.at(i).running=false; 690 } 691 } 692 693 if(nb_err!=logs.size()) is_logging=true; 694 } 695 else 696 { 697 Warn("Already logging\n"); 698 } 699 } 700 701 void FrameworkManager_impl::StopLog(void) 702 { 703 ssize_t written; 704 705 if(is_logging==true) 706 { 707 for(size_t i=0;i<logs.size();i++) 708 { 709 logs.at(i).running=false; 710 } 711 //send only one running false condition, user thread will stop and send all 712 #ifdef __XENO__ 713 written=rt_pipe_write(&cmd_pipe,&logs.at(0),sizeof(log_desc_t),P_NORMAL); 714 #else 715 written=write(cmd_pipe[1],&logs.at(0),sizeof(log_desc_t)); 716 #endif 717 718 if(written<0) 719 { 720 Err("write pipe error (%s)\n",strerror(-written)); 721 return; 722 } 723 else if (written != sizeof(log_desc_t)) 724 { 725 Err("write pipe error %ld/%ld\n",written,sizeof(log_desc_t)); 726 return; 727 } 728 729 is_logging=false; 730 } 731 else 732 { 733 Warn("Not logging\n"); 734 } 735 } 736 737 char* FrameworkManager_impl::GetBuffer(size_t sz) 738 { 739 //Printf("alloc %i\n",sz); 740 #ifdef __XENO__ 741 void *ptr; 742 int status=rt_heap_alloc(&log_heap,sz,TM_NONBLOCK ,&ptr); 743 if(status!=0) 744 { 745 Err("rt_heap_alloc error (%s)\n",strerror(-status)); 746 ptr=NULL; 747 } 748 return (char*)ptr; 749 #else 750 return (char*)malloc(sz); 751 #endif 752 } 753 754 void FrameworkManager_impl::ReleaseBuffer(char* buf) 755 { 756 #ifdef __XENO__ 757 int status=rt_heap_free(&log_heap,buf); 758 759 if(status!=0) 760 { 761 Err("rt_heap_free error (%s)\n",strerror(-status)); 762 } 763 #else 764 free(buf); 765 #endif 766 } 767 768 void FrameworkManager_impl::WriteLog(const char* buf,size_t size) 769 { 770 ssize_t written; 771 772 #ifdef __XENO__ 773 written=rt_pipe_write(&data_pipe,buf,size,P_NORMAL); 774 #else 775 written=write(data_pipe[1],buf,size); 776 #endif 777 778 if(written<0) 779 { 780 Err("erreur write pipe (%s)\n",strerror(-written)); 781 } 782 else if (written != (ssize_t)size) 783 { 784 Err("erreur write pipe %ld/%ld\n",written,size); 785 } 786 } 787 788 void* FrameworkManager_impl::write_log_user(void * arg) 789 { 790 int cmd_pipe=-1; 791 int data_pipe=-1; 792 string filename; 793 fd_set set; 794 struct timeval timeout; 795 FrameworkManager_impl *caller = (FrameworkManager_impl*)arg; 796 int rv; 797 798 vector<log_desc_t> logs; 799 800 #ifdef __XENO__ 801 filename=NRT_PIPE_PATH+ caller->self->ObjectName() + "-log_cmd"; 802 while(cmd_pipe<0) 803 { 804 cmd_pipe = open(filename.c_str(), O_RDWR); 805 if (cmd_pipe < 0 && errno!=ENOENT) caller->self->Err("open rt_pipe error: %s %i\n",filename.c_str(),errno); 806 usleep(1000); 807 } 808 filename=NRT_PIPE_PATH+ caller->self->ObjectName() + "-log_data"; 809 while(data_pipe<0) 810 { 811 data_pipe = open(filename.c_str(), O_RDWR); 812 if (data_pipe < 0 && errno!=ENOENT) caller->self->Err("open rt_pipe error: %s %i\n",filename.c_str(),errno); 813 usleep(1000); 814 } 815 #else 816 cmd_pipe=caller->cmd_pipe[0]; 817 data_pipe=caller->data_pipe[0]; 818 #endif 819 820 while(caller->continuer==true) 821 { 822 FD_ZERO(&set); 823 FD_SET(cmd_pipe, &set); 824 FD_SET(data_pipe, &set); 825 826 timeout.tv_sec = 0; 827 timeout.tv_usec = SELECT_TIMEOUT_MS*1000; 828 rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout); 829 830 if(rv == -1) 831 { 832 caller->Err("select error\n"); // an error accured 833 } 834 else if(rv == 0) 835 { 836 //printf("timeout write_log_user %s\n",caller->ObjectName().c_str()); // a timeout occured 837 } 838 else 839 { 840 if(FD_ISSET(cmd_pipe, &set)) 841 { 842 log_desc_t tmp; 843 read(cmd_pipe,&tmp,sizeof(log_desc_t)); 844 845 if(tmp.running==true)//start logging 846 { 847 filename= caller->log_path + "/" +caller->FileName(tmp.device)+ ".dbt"; 848 printf("Creating log file %s (log size %i)\n",filename.c_str(), (int)tmp.size); 849 tmp.dbtFile = inithdFile((char*)filename.c_str(),UAV,tmp.size); 850 logs.push_back(tmp); 851 852 if(logs.size()==1) 853 { 854 filename= caller->log_path + "/setup.xml"; 855 xmlSaveFile(filename.c_str(),caller->file_doc); 856 } 857 } 858 else//stop logging 859 { 860 for(size_t i=0;i<logs.size();i++) 861 { 862 if(logs.at(i).dbtFile!=NULL) 863 { 864 close_hdfile(logs.at(i).dbtFile); 865 866 filename= caller->FileName(logs.at(i).device)+ ".dbt"; 867 caller->SendFile(caller->log_path,filename); 868 869 fstream txt_file; 870 filename= caller->FileName(logs.at(i).device)+ ".txt"; 871 txt_file.open((caller->log_path+"/"+filename).c_str(),fstream::out); 872 txt_file << "1: time (us)\n2: time (ns)\n"; 873 int index=3; 874 logs.at(i).device->pimpl_->WriteLogsDescriptors(txt_file,&index); 875 txt_file.close(); 876 877 caller->SendFile(caller->log_path,filename); 878 } 879 } 880 //a revoir celui ci est le xml enregistrĂ© et pas forcement l'actuel 881 //if(caller->xml_file!="") caller->SendFile(caller->xml_file); 882 caller->FinishSending(); 883 884 logs.clear(); 885 } 886 } 887 888 if(FD_ISSET(data_pipe, &set)) 889 { 890 log_header_t header; 891 read(data_pipe,&header,sizeof(log_header_t)); 892 893 for(size_t i=0;i<logs.size();i++) 894 { 895 if(logs.at(i).device==header.device) 896 { 897 char* buf=(char*)malloc(header.size); 898 read(data_pipe,buf,header.size); 899 //printf("%s \n",header.device->ObjectName().c_str()); 900 //for(int i=0;i<header.size;i++) printf("%x ",buf[i]); 901 //printf("\n"); 902 if(logs.at(i).size==header.size) 903 { 904 if(logs.at(i).dbtFile!=NULL) 905 { 906 write_hdfile(logs.at(i).dbtFile,buf,(road_time_t)(header.time/1000),(road_timerange_t)(header.time%1000),header.size); 907 } 908 else 909 { 910 printf("err\n"); 911 } 912 } 913 else 914 { 915 caller->self->Err("%s log size is not correct %i/%i\n",header.device->ObjectName().c_str(),header.size,logs.at(i).size); 916 } 917 free(buf); 918 } 919 } 920 } 921 } 922 } 923 924 #ifdef __XENO__ 925 close(cmd_pipe); 926 close(data_pipe); 927 #endif 928 929 pthread_exit(0); 930 931 } 932 933 string FrameworkManager_impl::FileName(IODevice* device) 934 { 935 return getFrameworkManager()->ObjectName() + "_"+device->ObjectName(); 936 } 937 938 void FrameworkManager_impl::PrintXml(void) const 939 { 940 xmlChar *xmlbuff; 941 int buffersize; 942 xmlDocDumpFormatMemory(file_doc, &xmlbuff, &buffersize, 1); 943 Printf("xml:\n%s\n",xmlbuff); 944 xmlFree(xmlbuff); 945 } 869 } 870 } 871 } 872 873 #ifdef __XENO__ 874 close(cmd_pipe); 875 close(data_pipe); 876 #endif 877 878 pthread_exit(0); 879 } 880 881 string FrameworkManager_impl::FileName(IODevice *device) { 882 return getFrameworkManager()->ObjectName() + "_" + device->ObjectName(); 883 } 884 885 void FrameworkManager_impl::PrintXml(void) const { 886 xmlChar *xmlbuff; 887 int buffersize; 888 xmlDocDumpFormatMemory(file_doc, &xmlbuff, &buffersize, 1); 889 Printf("xml:\n%s\n", xmlbuff); 890 xmlFree(xmlbuff); 891 } -
trunk/lib/FlairCore/src/GeoCoordinate.cpp
r2 r15 20 20 using std::string; 21 21 22 namespace flair 23 { 24 namespace core 25 { 22 namespace flair { 23 namespace core { 26 24 27 GeoCoordinate::GeoCoordinate(const Object* parent,string name,const GeoCoordinate *point,int n): io_data(parent,name,n) 28 { 29 if(n>1) Warn("n>1 not supported\n"); 30 CopyFrom(point); 25 GeoCoordinate::GeoCoordinate(const Object *parent, string name, 26 const GeoCoordinate *point, int n) 27 : io_data(parent, name, n) { 28 if (n > 1) 29 Warn("n>1 not supported\n"); 30 CopyFrom(point); 31 31 } 32 32 33 GeoCoordinate::GeoCoordinate(const Object* parent,string name,double latitude,double longitude,double altitude,int n): io_data(parent,name,n) 34 { 35 this->latitude=latitude; 36 this->longitude=longitude; 37 this->altitude=altitude; 33 GeoCoordinate::GeoCoordinate(const Object *parent, string name, double latitude, 34 double longitude, double altitude, int n) 35 : io_data(parent, name, n) { 36 this->latitude = latitude; 37 this->longitude = longitude; 38 this->altitude = altitude; 38 39 } 39 40 40 GeoCoordinate::~GeoCoordinate() 41 { 41 GeoCoordinate::~GeoCoordinate() {} 42 42 43 void GeoCoordinate::CopyFrom(const GeoCoordinate *point) { 44 double latitude, longitude, altitude; 45 point->GetCoordinates(&latitude, &longitude, &altitude); 46 GetMutex(); 47 this->latitude = latitude; 48 this->longitude = longitude; 49 this->altitude = altitude; 50 ReleaseMutex(); 43 51 } 44 52 45 void GeoCoordinate::CopyFrom(const GeoCoordinate *point) 46 { 47 double latitude,longitude,altitude; 48 point->GetCoordinates(&latitude,&longitude,&altitude); 49 GetMutex(); 50 this->latitude=latitude; 51 this->longitude=longitude; 52 this->altitude=altitude; 53 ReleaseMutex(); 53 void GeoCoordinate::SetCoordinates(double latitude, double longitude, 54 double altitude) { 55 GetMutex(); 56 this->latitude = latitude; 57 this->longitude = longitude; 58 this->altitude = altitude; 59 ReleaseMutex(); 54 60 } 55 61 56 void GeoCoordinate:: SetCoordinates(double latitude,double longitude,double altitude)57 {58 59 this->latitude=latitude;60 this->longitude=longitude;61 this->altitude=altitude;62 62 void GeoCoordinate::GetCoordinates(double *latitude, double *longitude, 63 double *altitude) const { 64 GetMutex(); 65 *latitude = this->latitude; 66 *longitude = this->longitude; 67 *altitude = this->altitude; 68 ReleaseMutex(); 63 69 } 64 70 65 void GeoCoordinate::GetCoordinates(double *latitude,double *longitude,double *altitude) const 66 { 67 GetMutex(); 68 *latitude=this->latitude; 69 *longitude=this->longitude; 70 *altitude=this->altitude; 71 ReleaseMutex(); 72 } 73 74 void GeoCoordinate::CopyDatas(char* ptr) const 75 { 76 Warn("a ecrire"); 77 } 71 void GeoCoordinate::CopyDatas(char *ptr) const { Warn("a ecrire"); } 78 72 79 73 } // end namespace core -
trunk/lib/FlairCore/src/GeoCoordinate.h
r2 r15 15 15 #include <io_data.h> 16 16 17 namespace flair { namespace core { 17 namespace flair { 18 namespace core { 18 19