Changeset 122 in flair-src for trunk/demos/SimpleFleet/uav/src

Timestamp:

Jan 6, 2017, 1:56:26 PM (8 years ago)

Author:

Sanahuja Guillaume

Message:

modifs uav vrpn i686

Location:

trunk/demos/SimpleFleet/uav/src

Files:

• SimpleFleet.cpp (modified) (7 diffs)
• SimpleFleet.h (modified) (3 diffs)
• main.cpp (modified) (1 diff)

trunk/demos/SimpleFleet/uav/src/SimpleFleet.cpp

@@ -50 +50 @@
 
 
-SimpleFleet::SimpleFleet(flair::meta::Uav* uav,string broadcast,TargetController *controller): UavStateMachine(uav,controller), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
-    uav->SetupVRPNAutoIP(uav->ObjectName());
-
-    circle=new TrajectoryGenerator2DCircle(uav->GetVrpnClient()->GetLayout()->NewRow(),"circle");
-    uav->GetVrpnObject()->xPlot()->AddCurve(circle->Matrix()->Element(0,0),0,0,255);
-    uav->GetVrpnObject()->yPlot()->AddCurve(circle->Matrix()->Element(0,1),0,0,255);
-    uav->GetVrpnObject()->VxPlot()->AddCurve(circle->Matrix()->Element(1,0),0,0,255);
-    uav->GetVrpnObject()->VyPlot()->AddCurve(circle->Matrix()->Element(1,1),0,0,255);
-
-    xCircleCenter=new DoubleSpinBox(uav->GetVrpnClient()->GetLayout()->NewRow(),"x circle center"," m",-5,5,0.1,1,0);
-    yCircleCenter=new DoubleSpinBox(uav->GetVrpnClient()->GetLayout()->NewRow(),"y circle center"," m",-5,5,0.1,1,0);
-    yDisplacement=new DoubleSpinBox(uav->GetVrpnClient()->GetLayout()->NewRow(),"y displacement"," m",0,2,0.1,1,0);
+SimpleFleet::SimpleFleet(string broadcast,TargetController *controller): UavStateMachine(controller), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
+    Uav* uav=GetUav();
+                
+                VrpnClient* vrpnclient=new VrpnClient("vrpn", uav->GetDefaultVrpnAddress(),10000, 80);
+                uavVrpn = new MetaVrpnObject(uav->ObjectName());
+                getFrameworkManager()->AddDeviceToLog(uavVrpn);
+                uav->GetAhrs()->YawPlot()->AddCurve(uavVrpn->State()->Element(2),DataPlot::Green);
+
+    circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle");
+    uavVrpn->xPlot()->AddCurve(circle->Matrix()->Element(0,0),0,0,255);
+    uavVrpn->yPlot()->AddCurve(circle->Matrix()->Element(0,1),0,0,255);
+    uavVrpn->VxPlot()->AddCurve(circle->Matrix()->Element(1,0),0,0,255);
+    uavVrpn->VyPlot()->AddCurve(circle->Matrix()->Element(1,1),0,0,255);
+
+    xCircleCenter=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"x circle center"," m",-5,5,0.1,1,0);
+    yCircleCenter=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"y circle center"," m",-5,5,0.1,1,0);
+    yDisplacement=new DoubleSpinBox(vrpnclient->GetLayout()->NewRow(),"y displacement"," m",0,2,0.1,1,0);
 
     //parent->AddDeviceToLog(Uz());
@@ -101 +106 @@
     //get yaw from vrpn
     Euler vrpnEuler;
-    GetUav()->GetVrpnObject()->GetEuler(vrpnEuler);
+    uavVrpn->GetEuler(vrpnEuler);
 
     //get roll, pitch and w from imu
@@ -120 +125 @@
     Vector3D uav_pos,uav_vel;
 
-    GetUav()->GetVrpnObject()->GetPosition(uav_pos);
-    GetUav()->GetVrpnObject()->GetSpeed(uav_vel);
+    uavVrpn->GetPosition(uav_pos);
+    uavVrpn->GetSpeed(uav_vel);
     //z and dz must be in uav's frame
     z=-uav_pos.z;
@@ -154 +159 @@
     Euler vrpn_euler; // in VRPN coordinate system
 
-    GetUav()->GetVrpnObject()->GetPosition(uav_pos);
-    GetUav()->GetVrpnObject()->GetSpeed(uav_vel);
-    GetUav()->GetVrpnObject()->GetEuler(vrpn_euler);
+    uavVrpn->GetPosition(uav_pos);
+    uavVrpn->GetSpeed(uav_vel);
+    uavVrpn->GetEuler(vrpn_euler);
 
     uav_pos.To2Dxy(uav_2Dpos);
@@ -294 +299 @@
 void SimpleFleet::ExtraSecurityCheck(void) {
     if (!vrpnLost && behaviourMode!=BehaviourMode_t::Default) {
-        if (!GetUav()->GetVrpnObject()->IsTracked(500)) {
+        if (!uavVrpn->IsTracked(500)) {
             Thread::Err("Optitrack, uav lost\n");
             vrpnLost=true;
@@ -322 +327 @@
     circle->SetCenter(target_2Dpos);
 
-    GetUav()->GetVrpnObject()->GetPosition(uav_pos);
+    uavVrpn->GetPosition(uav_pos);
     uav_pos.To2Dxy(uav_2Dpos);
     circle->StartTraj(uav_2Dpos,1);
@@ -347 +352 @@
     }
 
-    GetUav()->GetVrpnObject()->GetEuler(vrpn_euler);
+    uavVrpn->GetEuler(vrpn_euler);
     yaw_hold=vrpn_euler.yaw;
 
-    GetUav()->GetVrpnObject()->GetPosition(vrpn_pos);
+    uavVrpn->GetPosition(vrpn_pos);
     vrpn_pos.To2Dxy(pos_hold);
 

trunk/demos/SimpleFleet/uav/src/SimpleFleet.h

@@ -26 +26 @@
         class TrajectoryGenerator2DCircle;
     }
+                namespace meta {
+        class MetaVrpnObject;
+    }
     namespace gui {
         class DoubleSpinBox;
@@ -34 +37 @@
 class SimpleFleet : public flair::meta::UavStateMachine {
     public:
-        SimpleFleet(flair::meta::Uav* uav,std::string broadcast,flair::sensor::TargetController *controller);
+        SimpleFleet(std::string broadcast,flair::sensor::TargetController *controller);
         ~SimpleFleet();
 
@@ -75 +78 @@
         flair::gui::DoubleSpinBox *xCircleCenter,*yCircleCenter,*yDisplacement;
         flair::core::AhrsData *customReferenceOrientation,*customOrientation;
+                                flair::meta::MetaVrpnObject *uavVrpn;
 };
 

trunk/demos/SimpleFleet/uav/src/main.cpp

@@ -46 +46 @@
     manager->SetupUserInterface(xml_file);
 
-    Uav* drone=CreateUav(manager,name,uav_type);
+    Uav* drone=CreateUav(name,uav_type);
     TargetEthController *controller=new TargetEthController(manager,"Dualshock3",ds3port);
-    SimpleFleet* demo=new SimpleFleet(drone,broadcast,controller);
+    SimpleFleet* demo=new SimpleFleet(broadcast,controller);
 
     demo->Start();