Changeset 135 in flair-src for trunk/lib/FlairSensorActuator

Timestamp:

02/13/17 11:38:33 (7 years ago)

Author:

Sanahuja Guillaume

Message:

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

Location:

trunk/lib/FlairSensorActuator/src

Files:

• VrpnObject.cpp (modified) (1 diff)
• VrpnObject.h (modified) (5 diffs)
• VrpnObject_impl.cpp (modified) (5 diffs)
• unexported/VrpnObject_impl.h (modified) (3 diffs)

trunk/lib/FlairSensorActuator/src/VrpnObject.cpp

@@ -67 +67 @@
 }
 
-void VrpnObject::GetEuler(Euler &euler) const { pimpl_->GetEuler(euler); }
-
 void VrpnObject::GetQuaternion(Quaternion &quaternion) const {
   pimpl_->GetQuaternion(quaternion);

trunk/lib/FlairSensorActuator/src/VrpnObject.h

@@ -22 +22 @@
                 class cvmatrix;
                 class Vector3D;
-                class Euler;
                 class Quaternion;
         }
@@ -101 +100 @@
 
   /*!
-  * \brief Get Euler angles
-  *
-  * \param euler output datas
-  */
-  void GetEuler(core::Euler &euler) const;
-
-  /*!
   * \brief Get Quaternion
   *
@@ -124 +116 @@
   *
   * Matrix is of type float and as follows: \n
-  * (0,0) roll (rad) \n
-  * (1,0) pitch (rad) \n
-  * (2,0) yaw (rad) \n
-  * (3,0) x \n
-  * (4,0) y \n
-  * (5,0) z \n
+  * (0,0) q0 \n
+  * (1,0) q1 \n
+  * (2,0) q2 \n
+        * (3,0) q3 \n
+  * (4,0) x \n
+  * (5,0) y \n
+  * (6,0) z \n
   *
-  * \return Output metrix
+  * \return Output matrix
   */
   core::cvmatrix *Output(void) const;
@@ -138 +131 @@
   * \brief State matrix
   *
+        * Used for plotting on ground station. \n
   * Matrix is of type float and as follows: \n
   * (0,0) roll (deg) \n
@@ -143 +137 @@
   * (2,0) yaw (deg) \n
   *
-  * \return State metrix
+  * \return State matrix
   */
   core::cvmatrix *State(void) const;

trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp

@@ -67 +67 @@
 
   // state
-  cvmatrix_descriptor *desc = new cvmatrix_descriptor(6, 1);
-  desc->SetElementName(0, 0, "roll");
-  desc->SetElementName(1, 0, "pitch");
-  desc->SetElementName(2, 0, "yaw");
-  desc->SetElementName(3, 0, "x");
-  desc->SetElementName(4, 0, "y");
-  desc->SetElementName(5, 0, "z");
+  cvmatrix_descriptor *desc = new cvmatrix_descriptor(7, 1);
+  desc->SetElementName(0, 0, "q0");
+  desc->SetElementName(1, 0, "q1");
+  desc->SetElementName(2, 0, "q2");
+  desc->SetElementName(3, 0, "q3");
+  desc->SetElementName(4, 0, "x");
+  desc->SetElementName(5, 0, "y");
+  desc->SetElementName(6, 0, "z");
   output = new cvmatrix(self, desc, floatType);
 
@@ -85 +86 @@
   plot_tab = new Tab(tab, "Mesures " + name);
   x_plot = new DataPlot1D(plot_tab->NewRow(), "x", -10, 10);
-  x_plot->AddCurve(output->Element(3));
+  x_plot->AddCurve(output->Element(4));
   y_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "y", -10, 10);
-  y_plot->AddCurve(output->Element(4));
+  y_plot->AddCurve(output->Element(5));
   z_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "z", -2, 0);
-  z_plot->AddCurve(output->Element(5));
+  z_plot->AddCurve(output->Element(6));
 }
 
@@ -122 +123 @@
 }
 
-void VrpnObject_impl::GetEuler(Euler &euler) {
-  output->GetMutex();
-  euler.roll = output->ValueNoMutex(0, 0);
-  euler.pitch = output->ValueNoMutex(1, 0);
-  euler.yaw = output->ValueNoMutex(2, 0);
-  output->ReleaseMutex();
-}
-
 void VrpnObject_impl::GetQuaternion(Quaternion &quaternion) {
   output->GetMutex();
-  quaternion.q0 = this->quaternion.q0;
-  quaternion.q1 = this->quaternion.q1;
-  quaternion.q2 = this->quaternion.q2;
-  quaternion.q3 = this->quaternion.q3;
+  quaternion.q0 = output->ValueNoMutex(0, 0);
+  quaternion.q1 = output->ValueNoMutex(1, 0);
+  quaternion.q2 = output->ValueNoMutex(2, 0);
+  quaternion.q3 = output->ValueNoMutex(3, 0);
   output->ReleaseMutex();
 }
@@ -141 +134 @@
 void VrpnObject_impl::GetPosition(Vector3D &point) {
   output->GetMutex();
-  point.x = output->ValueNoMutex(3, 0);
-  point.y = output->ValueNoMutex(4, 0);
-  point.z = output->ValueNoMutex(5, 0);
+  point.x = output->ValueNoMutex(4, 0);
+  point.y = output->ValueNoMutex(5, 0);
+  point.z = output->ValueNoMutex(6, 0);
   output->ReleaseMutex();
 }
@@ -172 +165 @@
   // warning: t.quat is defined as (qx,qy,qz,qw), which is different from
   // flair::core::Quaternion
-  caller->quaternion.q0 = t.quat[3];
-  caller->quaternion.q1 = t.quat[0];
-  caller->quaternion.q2 = t.quat[1];
-  caller->quaternion.q3 = t.quat[2];
+        Quaternion quaternion(t.quat[3],t.quat[0],t.quat[1],t.quat[2]);
   Vector3D pos((float)t.pos[0], (float)t.pos[1], (float)t.pos[2]);
 
   // on effectue les rotation
   caller->parent->pimpl_->ComputeRotations(pos);
-  caller->parent->pimpl_->ComputeRotations(caller->quaternion);
+  caller->parent->pimpl_->ComputeRotations(quaternion);
 
-  Euler euler;
-  caller->quaternion.ToEuler(euler);
-  caller->output->SetValueNoMutex(0, 0, euler.roll);
-  caller->output->SetValueNoMutex(1, 0, euler.pitch);
-  caller->output->SetValueNoMutex(2, 0, euler.yaw);
-  caller->output->SetValueNoMutex(3, 0, pos.x);
-  caller->output->SetValueNoMutex(4, 0, pos.y);
-  caller->output->SetValueNoMutex(5, 0, pos.z);
+  caller->output->SetValueNoMutex(0, 0, quaternion.q0);
+  caller->output->SetValueNoMutex(1, 0, quaternion.q1);
+  caller->output->SetValueNoMutex(2, 0, quaternion.q2);
+  caller->output->SetValueNoMutex(3, 0, quaternion.q3);
+  caller->output->SetValueNoMutex(4, 0, pos.x);
+  caller->output->SetValueNoMutex(5, 0, pos.y);
+  caller->output->SetValueNoMutex(6, 0, pos.z);
 
   caller->output->SetDataTime(time);
   caller->output->ReleaseMutex();
 
+        Euler euler=quaternion.ToEuler();
   caller->state->GetMutex();
   caller->state->SetValueNoMutex(0, 0, Euler::ToDegree(euler.roll));

trunk/lib/FlairSensorActuator/src/unexported/VrpnObject_impl.h

@@ -29 +29 @@
                 class cvmatrix;
                 class Vector3D;
-                class Euler;
         }
         namespace gui {
@@ -52 +51 @@
 
   void mainloop(void);
-  void GetEuler(flair::core::Euler &euler);
   void GetQuaternion(flair::core::Quaternion &quaternion);
   void GetPosition(flair::core::Vector3D &point);
@@ -69 +67 @@
   const flair::sensor::VrpnClient *parent;
   vrpn_Tracker_Remote *tracker;
-  flair::core::Quaternion quaternion; // todo: quaternion should be included in
-                                      // the output to replace euler angles
   void Update(void);
 };