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

Timestamp:

01/06/17 13:56:26 (7 years ago)

Author:

Sanahuja Guillaume

Message:

modifs uav vrpn i686

File:

• trunk/demos/SimpleFleet/uav/src/SimpleFleet.cpp (modified) (7 diffs)

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);