Changeset 45 in flair-src for trunk/lib/FlairMeta/src

Timestamp:

Jul 18, 2016, 4:13:56 PM (8 years ago)

Author:

Sanahuja Guillaume

Message:

added compile info

Location:

trunk/lib/FlairMeta/src

Files:

• UavFactory.cpp (modified) (4 diffs)
• UavStateMachine.cpp (modified) (2 diffs)
• UavStateMachine.h (modified) (1 diff)

trunk/lib/FlairMeta/src/UavFactory.cpp

@@ -18 +18 @@
 #include "UavFactory.h"
 #include "FrameworkManager.h"
-//#include "ArDrone2.h"
 #include "SimuX4.h"
 #include "SimuX8.h"
 #include "HdsX8.h"
 #include "XAir.h"
+#include "compile_info.h"
 
 using namespace std;
@@ -30 +30 @@
 
 namespace { // anonymous
-     vector<flair::meta::Uav* (*)(FrameworkManager*,string,string,UavMultiplex*)> vectoroffunctions;
+     vector<flair::meta::Uav* (*)(FrameworkManager*,string,string,UavMultiplex*)> *vectoroffunctions=NULL;
 }
+
+
+static void constructor() __attribute__((constructor));
+
+void constructor() {
+  compile_info("FlairMeta");
+}
+
 
 Uav *CreateUav(FrameworkManager *parent, string uav_name, string uav_type,
@@ -37 +45 @@
 
   Uav *uav;
-  for(int i=0;i<vectoroffunctions.size();i++) {
-    uav=vectoroffunctions.at(i)(parent, uav_name, uav_type,multiplex);
-    if(uav!=NULL) return uav;
+
+  if(vectoroffunctions!=NULL) {
+    for(int i=0;i<vectoroffunctions->size();i++) {
+      uav=vectoroffunctions->at(i)(parent, uav_name, uav_type,multiplex);
+      if(uav!=NULL) {
+        free(vectoroffunctions);
+        vectoroffunctions=NULL;
+        return uav;
+      }
+    }
   }
 
@@ -72 +87 @@
 void RegisterUavCreator(flair::meta::Uav*(*func)(FrameworkManager *parent, string uav_name, string uav_type,
                UavMultiplex *multiplex)) {
-                 vectoroffunctions.push_back(func);
+  if(vectoroffunctions==NULL) vectoroffunctions=(vector<flair::meta::Uav* (*)(FrameworkManager*,string,string,UavMultiplex*)>*)malloc(sizeof(vector<flair::meta::Uav* (*)(FrameworkManager*,string,string,UavMultiplex*)>));
 
+  vectoroffunctions->push_back(func);
 }

trunk/lib/FlairMeta/src/UavStateMachine.cpp

@@ -181 +181 @@
     ComputeThrust(); // logs are added to uz, so it must be updated at last
 
-    //check nan problems
-    if(CheckIsNan(currentTorques.roll,"roll torque")
-       || CheckIsNan(currentTorques.pitch,"pitch torque")
-       || CheckIsNan(currentTorques.yaw,"yaw torque")
-       || CheckIsNan(currentThrust,"thrust")) {
+    //check nan/inf problems
+    if(IsValuePossible(currentTorques.roll,"roll torque")
+       || IsValuePossible(currentTorques.pitch,"pitch torque")
+       || IsValuePossible(currentTorques.yaw,"yaw torque")
+       || IsValuePossible(currentThrust,"thrust")) {
 
       if(failSafeMode) {
@@ -218 +218 @@
 }
 
-bool UavStateMachine::CheckIsNan(float value,std::string desc) {
+bool UavStateMachine::IsValuePossible(float value,std::string desc) {
   if(isnan(value)) {
     Warn("%s is not an number\n",desc.c_str());
+    return true;
+  } else if(isinf(value)) {
+    Warn("%s is infinite\n",desc.c_str());
     return true;
   } else {

trunk/lib/FlairMeta/src/UavStateMachine.h

@@ -261 +261 @@
   void Run(void);
   void StopMotors(void);
-  bool CheckIsNan(float value,std::string desc);
+  bool IsValuePossible(float value,std::string desc);
 
   meta::Uav *uav;