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

Timestamp:

03/04/17 15:29:18 (7 years ago)

Author:

Sanahuja Guillaume

Message:

iadded isready to iodevice:
avoid problem of imu not ready in ardrone2

Location:

trunk/lib/FlairMeta/src

Files:

• HdsX8.cpp (modified) (2 diffs)
• HdsX8.h (modified) (1 diff)
• MetaDualShock3.cpp (modified) (1 diff)
• SimuX4.cpp (modified) (4 diffs)
• SimuX4.h (modified) (1 diff)
• SimuX8.cpp (modified) (2 diffs)
• SimuX8.h (modified) (1 diff)
• Uav.h (modified) (2 diffs)
• UavFactory.cpp (modified) (4 diffs)
• UavFactory.h (modified) (1 diff)
• UavStateMachine.cpp (modified) (2 diffs)
• XAir.cpp (modified) (2 diffs)
• XAir.h (modified) (1 diff)

trunk/lib/FlairMeta/src/HdsX8.cpp

@@ -35 +35 @@
 namespace meta {
 
-HdsX8::HdsX8(string name,
+HdsX8::HdsX8(string name,string options,
              filter::UavMultiplex *multiplex)
     : Uav(name, multiplex) {
@@ -56 +56 @@
 
 void HdsX8::StartSensors(void) {
-  ((Gx3_25_ahrs *)GetAhrs())->Start();
+  ((Gx3_25_imu *)(GetAhrs()->GetImu()))->Start();
   ((Srf08 *)GetUsRangeFinder())->Start();
   ((Ps3Eye *)GetVerticalCamera())->Start();
 }
 
+bool HdsX8::isReadyToFly(void) const {
+  return GetAhrs()->GetImu()->IsReady();
+}
+
 } // end namespace meta
 } // end namespace flair

trunk/lib/FlairMeta/src/HdsX8.h

@@ -24 +24 @@
 class HdsX8 : public Uav {
 public:
-  HdsX8(std::string name,
+  HdsX8(std::string name,std::string options="",
         filter::UavMultiplex *multiplex = NULL);
   ~HdsX8();
   void StartSensors(void);
         std::string GetDefaultVrpnAddress(void) const{return "192.168.147.197:3883";}
+  bool isReadyToFly(void) const;
 
 private:

trunk/lib/FlairMeta/src/MetaDualShock3.cpp

@@ -42 +42 @@
   getFrameworkManager()->AddDeviceToLog(pimpl_->joy_ref);
   controller->Start();
+  
+  SetIsReady(true);
 }
 

trunk/lib/FlairMeta/src/SimuX4.cpp

@@ -26 +26 @@
 #include <BatteryMonitor.h>
 #include <Tab.h>
+#include <FindArgument.h>
 
 using std::string;
@@ -37 +38 @@
 namespace meta {
 
-SimuX4::SimuX4(string name, int simu_id,
+SimuX4::SimuX4(string name, int simu_id,string options,
                filter::UavMultiplex *multiplex)
     : Uav(name, multiplex) {
@@ -51 +52 @@
   SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(), "battery"));
   GetBatteryMonitor()->SetBatteryValue(12);
+  
+  string camvOpts=FindArgument(options,"camv=",false);
+  uint16_t camvWidth=320,camvHeight=240;
+        if(camvOpts=="") {
+    Info("using default vertical camera resolution: %ix%i\n",camvWidth, camvHeight);
+        }
+  
   SetVerticalCamera(
-      new SimuCamera("simu_cam_v", 320, 240, 3, simu_id, 10));
+      new SimuCamera("simu_cam_v", camvWidth, camvHeight, 3, simu_id, 10));
 }
 
@@ -58 +66 @@
 
 void SimuX4::StartSensors(void) {
-  ((SimuAhrs *)GetAhrs())->Start();
+  ((SimuImu *)(GetAhrs()->GetImu()))->Start();
   ((SimuUs *)GetUsRangeFinder())->Start();
   ((SimuCamera *)GetVerticalCamera())->Start();
 }
 
+
 } // end namespace meta
 } // end namespace flair

trunk/lib/FlairMeta/src/SimuX4.h

@@ -26 +26 @@
   // simu_id: 0 if simulating only one UAV
   //>0 otherwise
-  SimuX4(std::string name, int simu_id = 0,
+  SimuX4(std::string name, int simu_id = 0,std::string options="",
          filter::UavMultiplex *multiplex = NULL);
   ~SimuX4();

trunk/lib/FlairMeta/src/SimuX8.cpp

@@ -37 +37 @@
 namespace meta {
 
-SimuX8::SimuX8(string name, int simu_id,
+SimuX8::SimuX8(string name, int simu_id,string options,
                filter::UavMultiplex *multiplex)
     : Uav(name, multiplex) {
@@ -58 +58 @@
 
 void SimuX8::StartSensors(void) {
-  ((SimuAhrs *)GetAhrs())->Start();
+  ((SimuImu *)(GetAhrs()->GetImu()))->Start();
   ((SimuUs *)GetUsRangeFinder())->Start();
   ((SimuCamera *)GetVerticalCamera())->Start();

trunk/lib/FlairMeta/src/SimuX8.h

@@ -26 +26 @@
   // simu_id: 0 if simulating only one UAV
   //>0 otherwise
-  SimuX8(std::string name, int simu_id = 0,
+  SimuX8(std::string name, int simu_id = 0,std::string options="",
          filter::UavMultiplex *multiplex = NULL);
   ~SimuX8();

trunk/lib/FlairMeta/src/Uav.h

@@ -15 +15 @@
 
 #include <Object.h>
+#include <UsRangeFinder.h>
 
 namespace flair {
@@ -62 +63 @@
   sensor::Camera *GetHorizontalCamera(void) const;
         virtual std::string GetDefaultVrpnAddress(void) const{return "127.0.0.1:3883";}
+  virtual bool isReadyToFly(void) const { return true;}
 
 protected:

trunk/lib/FlairMeta/src/UavFactory.cpp

@@ -30 +30 @@
 
 namespace { // anonymous
-     vector<flair::meta::Uav* (*)(string,string,UavMultiplex*)> *vectoroffunctions=NULL;
+     vector<flair::meta::Uav* (*)(string,string,string,UavMultiplex*)> *vectoroffunctions=NULL;
 }
 
@@ -41 +41 @@
 
 
-Uav *CreateUav(string name, string uav_type,
+Uav *CreateUav(string name, string type,string options,
                UavMultiplex *multiplex) {
 
@@ -48 +48 @@
   if(vectoroffunctions!=NULL) {
     for(int i=0;i<vectoroffunctions->size();i++) {
-      uav=vectoroffunctions->at(i)(name,uav_type,multiplex);
+      uav=vectoroffunctions->at(i)(name,type,options,multiplex);
       if(uav!=NULL) {
         free(vectoroffunctions);
@@ -57 +57 @@
   }
 
-  if (uav_type == "hds_x4") {
-    getFrameworkManager()->Err("UAV type %s not yet implemented\n", uav_type.c_str());
+  if (type == "hds_x4") {
+    getFrameworkManager()->Err("UAV type %s not yet implemented\n", type.c_str());
     return NULL;
-  } else if (uav_type == "hds_x8") {
-    return new HdsX8(name, multiplex);
-  } else if (uav_type == "xair") {
-    return new XAir(name, multiplex);
-  } else if (uav_type == "hds_xufo") {
-    getFrameworkManager()->Err("UAV type %s not yet implemented\n", uav_type.c_str());
+  } else if (type == "hds_x8") {
+    return new HdsX8(name,options, multiplex);
+  } else if (type == "xair") {
+    return new XAir(name,options, multiplex);
+  } else if (type == "hds_xufo") {
+    getFrameworkManager()->Err("UAV type %s not yet implemented\n", type.c_str());
     return NULL;
-  } else if (uav_type.compare(0, 7, "x4_simu") == 0) {
+  } else if (type.compare(0, 7, "x4_simu") == 0) {
     int simu_id = 0;
-    if (uav_type.size() > 7) {
-      simu_id = atoi(uav_type.substr(7, uav_type.size() - 7).c_str());
+    if (type.size() > 7) {
+      simu_id = atoi(type.substr(7, type.size() - 7).c_str());
     }
-    return new SimuX4(name, simu_id, multiplex);
-  } else if (uav_type.compare(0, 7, "x8_simu") == 0) {
+    return new SimuX4(name, simu_id,options, multiplex);
+  } else if (type.compare(0, 7, "x8_simu") == 0) {
     int simu_id = 0;
-    if (uav_type.size() > 7) {
-      simu_id = atoi(uav_type.substr(7, uav_type.size() - 7).c_str());
+    if (type.size() > 7) {
+      simu_id = atoi(type.substr(7, type.size() - 7).c_str());
     }
-    return new SimuX8(name, simu_id, multiplex);
+    return new SimuX8(name, simu_id,options, multiplex);
   } else {
-    getFrameworkManager()->Err("UAV type %s unknown\n", uav_type.c_str());
+    getFrameworkManager()->Err("UAV type %s unknown\n", type.c_str());
     return NULL;
   }
 }
 
-void RegisterUavCreator(flair::meta::Uav*(*func)(string name, string type,
-               UavMultiplex *multiplex)) {
-  if(vectoroffunctions==NULL) vectoroffunctions=(vector<flair::meta::Uav* (*)(string,string,UavMultiplex*)>*)malloc(sizeof(vector<flair::meta::Uav* (*)(string,string,UavMultiplex*)>));
+void RegisterUavCreator(flair::meta::Uav*(*func)(string,string,string,UavMultiplex*)) {
+  if(vectoroffunctions==NULL) vectoroffunctions=(vector<flair::meta::Uav* (*)(string,string,string,UavMultiplex*)>*)malloc(sizeof(vector<flair::meta::Uav* (*)(string,string,string,UavMultiplex*)>));
 
   vectoroffunctions->push_back(func);

trunk/lib/FlairMeta/src/UavFactory.h

@@ -21 +21 @@
 #include <Uav.h>
 
-flair::meta::Uav *CreateUav(std::string name, std::string type,
+flair::meta::Uav *CreateUav(std::string name, std::string type,std::string options="",
                             flair::filter::UavMultiplex *multiplex = NULL);
 
-void RegisterUavCreator(flair::meta::Uav*(*func)(std::string name, std::string type,
-                                   flair::filter::UavMultiplex *multiplex));
+void RegisterUavCreator(flair::meta::Uav*(*func)(std::string,std::string,std::string,
+                                   flair::filter::UavMultiplex*));
 #endif // UAVFACTORY

trunk/lib/FlairMeta/src/UavStateMachine.cpp

@@ -460 +460 @@
   flagZTrajectoryFinished = false;
 
-    if((altitudeState==AltitudeState_t::Stopped) && safeToFly) {// && GetBatteryMonitor()->IsBatteryLow()==false)
-        //The uav always takes off in fail safe mode
-        EnterFailSafeMode();
-        joy->SetLedOFF(4);//DualShock3::led4
-        joy->SetLedOFF(1);//DualShock3::led1
-        joy->Rumble(0x70);
-        joy->SetZRef(0);
+  if((altitudeState==AltitudeState_t::Stopped) && safeToFly && uav->isReadyToFly()) {// && GetBatteryMonitor()->IsBatteryLow()==false)
+    //The uav always takes off in fail safe mode
+    EnterFailSafeMode();
+    joy->SetLedOFF(4);//DualShock3::led4
+    joy->SetLedOFF(1);//DualShock3::led1
+    joy->Rumble(0x70);
+    joy->SetZRef(0);
 
     uZ->SetOffset(); // positionne l'offset de compensation de la gravité à sa
@@ -481 +481 @@
     SignalEvent(Event_t::TakingOff);
   } else {
+    Warn("cannot takeoff\n");
     joy->ErrorNotify();
   }

trunk/lib/FlairMeta/src/XAir.cpp

@@ -38 +38 @@
 namespace meta {
 
-XAir::XAir(string name,
+XAir::XAir(string name, string options,
            filter::UavMultiplex *multiplex)
     : Uav(name, multiplex) {
@@ -61 +61 @@
 
 void XAir::StartSensors(void) {
-  ((Gx3_25_ahrs *)GetAhrs())->Start();
+  ((Gx3_25_imu *)(GetAhrs()->GetImu()))->Start();
   ((Srf08 *)GetUsRangeFinder())->Start();
   ((Ps3Eye *)GetVerticalCamera())->Start();
 }
 
+bool XAir::isReadyToFly(void) const {
+  return GetAhrs()->GetImu()->IsReady();
+}
+
 } // end namespace meta
 } // end namespace flair

trunk/lib/FlairMeta/src/XAir.h

@@ -24 +24 @@
 class XAir : public Uav {
 public:
-  XAir(std::string name,
+  XAir(std::string name,std::string options="",
        filter::UavMultiplex *multiplex = NULL);
   ~XAir();
   void StartSensors(void);
         std::string GetDefaultVrpnAddress(void) const{return "192.168.147.197:3883";}
+  bool isReadyToFly(void) const;
 
 private: