Changeset 135 in flair-src for trunk/lib


Ignore:
Timestamp:
Feb 13, 2017, 11:38:33 AM (5 years ago)
Author:
Sanahuja Guillaume
Message:

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

Location:
trunk/lib
Files:
5 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairMeta/src/MetaVrpnObject.cpp

    r122 r135  
    5151
    5252void MetaVrpnObject::ConstructorCommon(string name) {
    53   cvmatrix_descriptor *desc = new cvmatrix_descriptor(6, 1);
    54   for (int i = 0; i < 6; i++) {
     53  cvmatrix_descriptor *desc = new cvmatrix_descriptor(7, 1);
     54  for (int i = 0; i < desc->Rows(); i++) {
    5555    desc->SetElementName(i, 0, Output()->Name(i, 0));
    5656  }
    5757  cvmatrix *prev_value = new cvmatrix(this, desc, elementDataType, name);
    58   for (int i = 0; i < 6; i++) {
     58  for (int i = 0; i < prev_value->Rows(); i++) {
    5959    prev_value->SetValue(i, 0, 0);
    6060  }
     
    6363                           name + " Passe bas", prev_value);
    6464
    65   desc = new cvmatrix_descriptor(6, 1);
    66   for (int i = 0; i < 6; i++) {
     65  desc = new cvmatrix_descriptor(7, 1);
     66  for (int i = 0; i < desc->Rows(); i++) {
    6767    desc->SetElementName(i, 0, "d" + Output()->Name(i, 0));
    6868  }
    6969  prev_value = new cvmatrix(this, desc, elementDataType, name);
    70   for (int i = 0; i < 6; i++) {
     70  for (int i = 0; i < prev_value->Rows(); i++) {
    7171    prev_value->SetValue(i, 0, 0);
    7272  }
     
    7676
    7777  vx_opti_plot = new DataPlot1D(GetPlotTab()->NewRow(), "vx", -3, 3);
    78   vx_opti_plot->AddCurve(euler->Matrix()->Element(3));
     78  vx_opti_plot->AddCurve(euler->Matrix()->Element(4));
    7979  vy_opti_plot = new DataPlot1D(GetPlotTab()->LastRowLastCol(), "vy", -3, 3);
    80   vy_opti_plot->AddCurve(euler->Matrix()->Element(4));
     80  vy_opti_plot->AddCurve(euler->Matrix()->Element(5));
    8181  vz_opti_plot = new DataPlot1D(GetPlotTab()->LastRowLastCol(), "vz", -2, 2);
    82   vz_opti_plot->AddCurve(euler->Matrix()->Element(5));
     82  vz_opti_plot->AddCurve(euler->Matrix()->Element(6));
    8383
    8484  plot_tab = new Tab(GetVrpnClient()->GetTabWidget(), "Mesures (xy) " + name);
    8585  xy_plot = new DataPlot2D(plot_tab->NewRow(), "xy", "y", -5, 5, "x", -5, 5);
    86   xy_plot->AddCurve(Output()->Element(4, 0), Output()->Element(3, 0));
     86  xy_plot->AddCurve(Output()->Element(5, 0), Output()->Element(4, 0));
    8787}
    8888
     
    9898
    9999void MetaVrpnObject::GetSpeed(Vector3D &speed) const {
    100   speed.x = euler->Output(3, 0);
    101   speed.y = euler->Output(4, 0);
    102   speed.z = euler->Output(5, 0);
     100  speed.x = euler->Output(4, 0);
     101  speed.y = euler->Output(5, 0);
     102  speed.z = euler->Output(6, 0);
    103103}
    104104
  • trunk/lib/FlairSensorActuator/src/VrpnObject.cpp

    r122 r135  
    6767}
    6868
    69 void VrpnObject::GetEuler(Euler &euler) const { pimpl_->GetEuler(euler); }
    70 
    7169void VrpnObject::GetQuaternion(Quaternion &quaternion) const {
    7270  pimpl_->GetQuaternion(quaternion);
  • trunk/lib/FlairSensorActuator/src/VrpnObject.h

    r122 r135  
    2222                class cvmatrix;
    2323                class Vector3D;
    24                 class Euler;
    2524                class Quaternion;
    2625        }
     
    101100
    102101  /*!
    103   * \brief Get Euler angles
    104   *
    105   * \param euler output datas
    106   */
    107   void GetEuler(core::Euler &euler) const;
    108 
    109   /*!
    110102  * \brief Get Quaternion
    111103  *
     
    124116  *
    125117  * Matrix is of type float and as follows: \n
    126   * (0,0) roll (rad) \n
    127   * (1,0) pitch (rad) \n
    128   * (2,0) yaw (rad) \n
    129   * (3,0) x \n
    130   * (4,0) y \n
    131   * (5,0) z \n
     118  * (0,0) q0 \n
     119  * (1,0) q1 \n
     120  * (2,0) q2 \n
     121        * (3,0) q3 \n
     122  * (4,0) x \n
     123  * (5,0) y \n
     124  * (6,0) z \n
    132125  *
    133   * \return Output metrix
     126  * \return Output matrix
    134127  */
    135128  core::cvmatrix *Output(void) const;
     
    138131  * \brief State matrix
    139132  *
     133        * Used for plotting on ground station. \n
    140134  * Matrix is of type float and as follows: \n
    141135  * (0,0) roll (deg) \n
     
    143137  * (2,0) yaw (deg) \n
    144138  *
    145   * \return State metrix
     139  * \return State matrix
    146140  */
    147141  core::cvmatrix *State(void) const;
  • trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp

    r122 r135  
    6767
    6868  // state
    69   cvmatrix_descriptor *desc = new cvmatrix_descriptor(6, 1);
    70   desc->SetElementName(0, 0, "roll");
    71   desc->SetElementName(1, 0, "pitch");
    72   desc->SetElementName(2, 0, "yaw");
    73   desc->SetElementName(3, 0, "x");
    74   desc->SetElementName(4, 0, "y");
    75   desc->SetElementName(5, 0, "z");
     69  cvmatrix_descriptor *desc = new cvmatrix_descriptor(7, 1);
     70  desc->SetElementName(0, 0, "q0");
     71  desc->SetElementName(1, 0, "q1");
     72  desc->SetElementName(2, 0, "q2");
     73  desc->SetElementName(3, 0, "q3");
     74  desc->SetElementName(4, 0, "x");
     75  desc->SetElementName(5, 0, "y");
     76  desc->SetElementName(6, 0, "z");
    7677  output = new cvmatrix(self, desc, floatType);
    7778
     
    8586  plot_tab = new Tab(tab, "Mesures " + name);
    8687  x_plot = new DataPlot1D(plot_tab->NewRow(), "x", -10, 10);
    87   x_plot->AddCurve(output->Element(3));
     88  x_plot->AddCurve(output->Element(4));
    8889  y_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "y", -10, 10);
    89   y_plot->AddCurve(output->Element(4));
     90  y_plot->AddCurve(output->Element(5));
    9091  z_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "z", -2, 0);
    91   z_plot->AddCurve(output->Element(5));
     92  z_plot->AddCurve(output->Element(6));
    9293}
    9394
     
    122123}
    123124
    124 void VrpnObject_impl::GetEuler(Euler &euler) {
    125   output->GetMutex();
    126   euler.roll = output->ValueNoMutex(0, 0);
    127   euler.pitch = output->ValueNoMutex(1, 0);
    128   euler.yaw = output->ValueNoMutex(2, 0);
    129   output->ReleaseMutex();
    130 }
    131 
    132125void VrpnObject_impl::GetQuaternion(Quaternion &quaternion) {
    133126  output->GetMutex();
    134   quaternion.q0 = this->quaternion.q0;
    135   quaternion.q1 = this->quaternion.q1;
    136   quaternion.q2 = this->quaternion.q2;
    137   quaternion.q3 = this->quaternion.q3;
     127  quaternion.q0 = output->ValueNoMutex(0, 0);
     128  quaternion.q1 = output->ValueNoMutex(1, 0);
     129  quaternion.q2 = output->ValueNoMutex(2, 0);
     130  quaternion.q3 = output->ValueNoMutex(3, 0);
    138131  output->ReleaseMutex();
    139132}
     
    141134void VrpnObject_impl::GetPosition(Vector3D &point) {
    142135  output->GetMutex();
    143   point.x = output->ValueNoMutex(3, 0);
    144   point.y = output->ValueNoMutex(4, 0);
    145   point.z = output->ValueNoMutex(5, 0);
     136  point.x = output->ValueNoMutex(4, 0);
     137  point.y = output->ValueNoMutex(5, 0);
     138  point.z = output->ValueNoMutex(6, 0);
    146139  output->ReleaseMutex();
    147140}
     
    172165  // warning: t.quat is defined as (qx,qy,qz,qw), which is different from
    173166  // flair::core::Quaternion
    174   caller->quaternion.q0 = t.quat[3];
    175   caller->quaternion.q1 = t.quat[0];
    176   caller->quaternion.q2 = t.quat[1];
    177   caller->quaternion.q3 = t.quat[2];
     167        Quaternion quaternion(t.quat[3],t.quat[0],t.quat[1],t.quat[2]);
    178168  Vector3D pos((float)t.pos[0], (float)t.pos[1], (float)t.pos[2]);
    179169
    180170  // on effectue les rotation
    181171  caller->parent->pimpl_->ComputeRotations(pos);
    182   caller->parent->pimpl_->ComputeRotations(caller->quaternion);
     172  caller->parent->pimpl_->ComputeRotations(quaternion);
    183173
    184   Euler euler;
    185   caller->quaternion.ToEuler(euler);
    186   caller->output->SetValueNoMutex(0, 0, euler.roll);
    187   caller->output->SetValueNoMutex(1, 0, euler.pitch);
    188   caller->output->SetValueNoMutex(2, 0, euler.yaw);
    189   caller->output->SetValueNoMutex(3, 0, pos.x);
    190   caller->output->SetValueNoMutex(4, 0, pos.y);
    191   caller->output->SetValueNoMutex(5, 0, pos.z);
     174  caller->output->SetValueNoMutex(0, 0, quaternion.q0);
     175  caller->output->SetValueNoMutex(1, 0, quaternion.q1);
     176  caller->output->SetValueNoMutex(2, 0, quaternion.q2);
     177  caller->output->SetValueNoMutex(3, 0, quaternion.q3);
     178  caller->output->SetValueNoMutex(4, 0, pos.x);
     179  caller->output->SetValueNoMutex(5, 0, pos.y);
     180  caller->output->SetValueNoMutex(6, 0, pos.z);
    192181
    193182  caller->output->SetDataTime(time);
    194183  caller->output->ReleaseMutex();
    195184
     185        Euler euler=quaternion.ToEuler();
    196186  caller->state->GetMutex();
    197187  caller->state->SetValueNoMutex(0, 0, Euler::ToDegree(euler.roll));
  • trunk/lib/FlairSensorActuator/src/unexported/VrpnObject_impl.h

    r122 r135  
    2929                class cvmatrix;
    3030                class Vector3D;
    31                 class Euler;
    3231        }
    3332        namespace gui {
     
    5251
    5352  void mainloop(void);
    54   void GetEuler(flair::core::Euler &euler);
    5553  void GetQuaternion(flair::core::Quaternion &quaternion);
    5654  void GetPosition(flair::core::Vector3D &point);
     
    6967  const flair::sensor::VrpnClient *parent;
    7068  vrpn_Tracker_Remote *tracker;
    71   flair::core::Quaternion quaternion; // todo: quaternion should be included in
    72                                       // the output to replace euler angles
    7369  void Update(void);
    7470};
Note: See TracChangeset for help on using the changeset viewer.