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

Timestamp:

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

Author:

Sanahuja Guillaume

Message:

modifs uav vrpn i686

File:

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

trunk/demos/CircleFollower/uav/src/CircleFollower.cpp

@@ -39 +39 @@
 using namespace flair::meta;
 
-CircleFollower::CircleFollower(Uav* uav,TargetController *controller): UavStateMachine(uav,controller), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
-    uav->SetupVRPNAutoIP(uav->ObjectName());
-
+CircleFollower::CircleFollower(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);
+                                                                                                                                 
     startCircle=new PushButton(GetButtonsLayout()->NewRow(),"start_circle");
     stopCircle=new PushButton(GetButtonsLayout()->LastRowLastCol(),"stop_circle");
 
-    if(uav->GetVrpnClient()->UseXbee()==true) {
-        targetVrpn=new MetaVrpnObject(uav->GetVrpnClient(),"target",1);
+    if(vrpnclient->UseXbee()==true) {
+        targetVrpn=new MetaVrpnObject("target",1);
     } else {
-        targetVrpn=new MetaVrpnObject(uav->GetVrpnClient(),"target");
+        targetVrpn=new MetaVrpnObject("target");
     }
 
     getFrameworkManager()->AddDeviceToLog(targetVrpn);
 
-    circle=new TrajectoryGenerator2DCircle(uav->GetVrpnClient()->GetLayout()->NewRow(),"circle");
-    uav->GetVrpnObject()->xPlot()->AddCurve(circle->Matrix()->Element(0,0),DataPlot::Blue);
-    uav->GetVrpnObject()->yPlot()->AddCurve(circle->Matrix()->Element(0,1),DataPlot::Blue);
-    uav->GetVrpnObject()->VxPlot()->AddCurve(circle->Matrix()->Element(1,0),DataPlot::Blue);
-    uav->GetVrpnObject()->VyPlot()->AddCurve(circle->Matrix()->Element(1,1),DataPlot::Blue);
-    uav->GetVrpnObject()->XyPlot()->AddCurve(circle->Matrix()->Element(0,1),circle->Matrix()->Element(0,0),DataPlot::Blue,"circle");
+    circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle");
+    uavVrpn->xPlot()->AddCurve(circle->Matrix()->Element(0,0),DataPlot::Blue);
+    uavVrpn->yPlot()->AddCurve(circle->Matrix()->Element(0,1),DataPlot::Blue);
+    uavVrpn->VxPlot()->AddCurve(circle->Matrix()->Element(1,0),DataPlot::Blue);
+    uavVrpn->VyPlot()->AddCurve(circle->Matrix()->Element(1,1),DataPlot::Blue);
+    uavVrpn->XyPlot()->AddCurve(circle->Matrix()->Element(0,1),circle->Matrix()->Element(0,0),DataPlot::Blue,"circle");
 
     uX=new Pid(setupLawTab->At(1,0),"u_x");
@@ -79 +84 @@
     //get yaw from vrpn
     Euler vrpnEuler;
-    GetUav()->GetVrpnObject()->GetEuler(vrpnEuler);
+    uavVrpn->GetEuler(vrpnEuler);
 
     //get roll, pitch and w from imu
@@ -98 +103 @@
     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;
@@ -131 +136 @@
     Vector2D uav_2Dpos,uav_2Dvel; // in VRPN coordinate system
 
-    GetUav()->GetVrpnObject()->GetPosition(uav_pos);
-    GetUav()->GetVrpnObject()->GetSpeed(uav_vel);
+    uavVrpn->GetPosition(uav_pos);
+    uavVrpn->GetSpeed(uav_vel);
 
     uav_pos.To2Dxy(uav_2Dpos);
@@ -195 +200 @@
             Land();
         }
-        if (!GetUav()->GetVrpnObject()->IsTracked(500)) {
+        if (!uavVrpn->IsTracked(500)) {
             Thread::Err("VRPN, uav lost\n");
             vrpnLost=true;
@@ -239 +244 @@
     circle->SetCenter(target_2Dpos);
 
-    GetUav()->GetVrpnObject()->GetPosition(uav_pos);
+    uavVrpn->GetPosition(uav_pos);
     uav_pos.To2Dxy(uav_2Dpos);
     circle->StartTraj(uav_2Dpos);
@@ -258 +263 @@
     Vector3D vrpn_pos;
 
-    GetUav()->GetVrpnObject()->GetEuler(vrpn_euler);
+    uavVrpn->GetEuler(vrpn_euler);
     yawHold=vrpn_euler.yaw;
 
-    GetUav()->GetVrpnObject()->GetPosition(vrpn_pos);
+    uavVrpn->GetPosition(vrpn_pos);
     vrpn_pos.To2Dxy(posHold);