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

Timestamp:

Feb 13, 2017, 11:38:33 AM (8 years ago)

Author:

Sanahuja Guillaume

Message:

VrpnObject no longer outputs Euler (ony Quaternion): warning, output matrix has changed!

Location:

trunk/demos/SimpleFleet/uav/src

Files:

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

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

@@ -106 +106 @@
 const AhrsData *SimpleFleet::GetOrientation(void) const {
     //get yaw from vrpn
-    Euler vrpnEuler;
-    uavVrpn->GetEuler(vrpnEuler);
+                Quaternion vrpnQuaternion;
+    uavVrpn->GetQuaternion(vrpnQuaternion);
 
     //get roll, pitch and w from imu
@@ -115 +115 @@
 
     Euler ahrsEuler=ahrsQuaternion.ToEuler();
-    ahrsEuler.yaw=vrpnEuler.yaw;
+    ahrsEuler.yaw=vrpnQuaternion.ToEuler().yaw;
     Quaternion mixQuaternion=ahrsEuler.ToQuaternion();
 
@@ -158 +158 @@
     Vector3D uav_pos,uav_vel; // in VRPN coordinate system
     Vector2D uav_2Dpos,uav_2Dvel; // in VRPN coordinate system
-    Euler vrpn_euler; // in VRPN coordinate system
 
     uavVrpn->GetPosition(uav_pos);
     uavVrpn->GetSpeed(uav_vel);
-    uavVrpn->GetEuler(vrpn_euler);
 
     uav_pos.To2Dxy(uav_2Dpos);
@@ -169 +167 @@
     if (behaviourMode==BehaviourMode_t::PositionHold1 || behaviourMode==BehaviourMode_t::PositionHold2
         || behaviourMode==BehaviourMode_t::PositionHold3 || behaviourMode==BehaviourMode_t::PositionHold4) {
-        pos_error=uav_2Dpos-pos_hold;
+        pos_error=uav_2Dpos-posHold;
         vel_error=uav_2Dvel;
-        yaw_ref=yaw_hold;
+        yaw_ref=yawHold;
     } else { //Circle
         Vector2D circle_pos,circle_vel;
@@ -222 +220 @@
             VrpnPositionHold();
             behaviourMode=BehaviourMode_t::PositionHold2;
-            if(pos_hold.y<0) {
-                pos_hold.y-=yDisplacement->Value();
+            if(posHold.y<0) {
+                posHold.y-=yDisplacement->Value();
             } else {
-                pos_hold.y+=yDisplacement->Value();
+                posHold.y+=yDisplacement->Value();
             }
             posWait=GetTime();
@@ -232 +230 @@
         if (behaviourMode==BehaviourMode_t::PositionHold2 && GetTime()>(posWait+3*(Time)1000000000)) {
             behaviourMode=BehaviourMode_t::PositionHold3;
-            if(pos_hold.y<0) {
-                pos_hold.y+=yDisplacement->Value();
+            if(posHold.y<0) {
+                posHold.y+=yDisplacement->Value();
             } else {
-                pos_hold.y-=yDisplacement->Value();
+                posHold.y-=yDisplacement->Value();
             }
             posWait=GetTime();
@@ -343 +341 @@
 
 void SimpleFleet::VrpnPositionHold(void) {
-    Euler vrpn_euler;
-    Vector3D vrpn_pos;
-
-    if (SetOrientationMode(OrientationMode_t::Custom)) {
+  if (SetOrientationMode(OrientationMode_t::Custom)) {
         Thread::Info("Demo flotte: holding position\n");
     } else {
         Thread::Info("Demo flotte: could not hold position\n");
         //return;
-    }
-
-    uavVrpn->GetEuler(vrpn_euler);
-    yaw_hold=vrpn_euler.yaw;
-
-    uavVrpn->GetPosition(vrpn_pos);
-    vrpn_pos.To2Dxy(pos_hold);
+    }  
+                
+                Quaternion vrpnQuaternion;
+    uavVrpn->GetQuaternion(vrpnQuaternion);
+                yawHold=vrpnQuaternion.ToEuler().yaw;
+
+                Vector3D vrpnPosition;
+    uavVrpn->GetPosition(vrpnPosition);
+    vrpnPosition.To2Dxy(posHold);
 
     u_x->Reset();

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

@@ -70 +70 @@
         flair::filter::Pid *u_x, *u_y;
 
-        flair::core::Vector2D pos_hold;
-        float yaw_hold;
+        flair::core::Vector2D posHold;
+        float yawHold;
         flair::core::Socket *message;
         flair::core::Time posWait;