Changeset 135 in flair-src for trunk


Ignore:
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
Files:
9 edited

Legend:

Unmodified
Added
Removed

  • trunk/ReleaseNotes

    r133 r135  
    1010- Uav class is now a singleton
    1111- VrpnClient class is now a singleton
     12- VrpnObject no longer outputs Euler (ony Quaternion): warning, output matrix has changed!
    1213
    1314-----------------------------------------------------------

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

    r129 r135  
    8484const AhrsData *CircleFollower::GetOrientation(void) const {
    8585    //get yaw from vrpn
    86     Euler vrpnEuler;
    87     uavVrpn->GetEuler(vrpnEuler);
     86                Quaternion vrpnQuaternion;
     87    uavVrpn->GetQuaternion(vrpnQuaternion);
    8888
    8989    //get roll, pitch and w from imu
    … …  
    9393
    9494    Euler ahrsEuler=ahrsQuaternion.ToEuler();
    95     ahrsEuler.yaw=vrpnEuler.yaw;
     95    ahrsEuler.yaw=vrpnQuaternion.ToEuler().yaw;
    9696    Quaternion mixQuaternion=ahrsEuler.ToQuaternion();
    9797
    … …  
    261261
    262262void CircleFollower::VrpnPositionHold(void) {
    263     Euler vrpn_euler;
    264     Vector3D vrpn_pos;
    265 
    266     uavVrpn->GetEuler(vrpn_euler);
    267     yawHold=vrpn_euler.yaw;
    268 
    269     uavVrpn->GetPosition(vrpn_pos);
    270     vrpn_pos.To2Dxy(posHold);
     263                Quaternion vrpnQuaternion;
     264    uavVrpn->GetQuaternion(vrpnQuaternion);
     265                yawHold=vrpnQuaternion.ToEuler().yaw;
     266
     267    Vector3D vrpnPosition;
     268    uavVrpn->GetPosition(vrpnPosition);
     269    vrpnPosition.To2Dxy(posHold);
    271270
    272271    uX->Reset();

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

    r129 r135  
    106106const AhrsData *SimpleFleet::GetOrientation(void) const {
    107107    //get yaw from vrpn
    108     Euler vrpnEuler;
    109     uavVrpn->GetEuler(vrpnEuler);
     108                Quaternion vrpnQuaternion;
     109    uavVrpn->GetQuaternion(vrpnQuaternion);
    110110
    111111    //get roll, pitch and w from imu
    … …  
    115115
    116116    Euler ahrsEuler=ahrsQuaternion.ToEuler();
    117     ahrsEuler.yaw=vrpnEuler.yaw;
     117    ahrsEuler.yaw=vrpnQuaternion.ToEuler().yaw;
    118118    Quaternion mixQuaternion=ahrsEuler.ToQuaternion();
    119119
    … …  
    158158    Vector3D uav_pos,uav_vel; // in VRPN coordinate system
    159159    Vector2D uav_2Dpos,uav_2Dvel; // in VRPN coordinate system
    160     Euler vrpn_euler; // in VRPN coordinate system
    161160
    162161    uavVrpn->GetPosition(uav_pos);
    163162    uavVrpn->GetSpeed(uav_vel);
    164     uavVrpn->GetEuler(vrpn_euler);
    165163
    166164    uav_pos.To2Dxy(uav_2Dpos);
    … …  
    169167    if (behaviourMode==BehaviourMode_t::PositionHold1 || behaviourMode==BehaviourMode_t::PositionHold2
    170168        || behaviourMode==BehaviourMode_t::PositionHold3 || behaviourMode==BehaviourMode_t::PositionHold4) {
    171         pos_error=uav_2Dpos-pos_hold;
     169        pos_error=uav_2Dpos-posHold;
    172170        vel_error=uav_2Dvel;
    173         yaw_ref=yaw_hold;
     171        yaw_ref=yawHold;
    174172    } else { //Circle
    175173        Vector2D circle_pos,circle_vel;
    … …  
    222220            VrpnPositionHold();
    223221            behaviourMode=BehaviourMode_t::PositionHold2;
    224             if(pos_hold.y<0) {
    225                 pos_hold.y-=yDisplacement->Value();
     222            if(posHold.y<0) {
     223                posHold.y-=yDisplacement->Value();
    226224            } else {
    227                 pos_hold.y+=yDisplacement->Value();
     225                posHold.y+=yDisplacement->Value();
    228226            }
    229227            posWait=GetTime();
    … …  
    232230        if (behaviourMode==BehaviourMode_t::PositionHold2 && GetTime()>(posWait+3*(Time)1000000000)) {
    233231            behaviourMode=BehaviourMode_t::PositionHold3;
    234             if(pos_hold.y<0) {
    235                 pos_hold.y+=yDisplacement->Value();
     232            if(posHold.y<0) {
     233                posHold.y+=yDisplacement->Value();
    236234            } else {
    237                 pos_hold.y-=yDisplacement->Value();
     235                posHold.y-=yDisplacement->Value();
    238236            }
    239237            posWait=GetTime();
    … …  
    343341
    344342void SimpleFleet::VrpnPositionHold(void) {
    345     Euler vrpn_euler;
    346     Vector3D vrpn_pos;
    347 
    348     if (SetOrientationMode(OrientationMode_t::Custom)) {
     343  if (SetOrientationMode(OrientationMode_t::Custom)) {
    349344        Thread::Info("Demo flotte: holding position\n");
    350345    } else {
    351346        Thread::Info("Demo flotte: could not hold position\n");
    352347        //return;
    353     }
    354 
    355     uavVrpn->GetEuler(vrpn_euler);
    356     yaw_hold=vrpn_euler.yaw;
    357 
    358     uavVrpn->GetPosition(vrpn_pos);
    359     vrpn_pos.To2Dxy(pos_hold);
     348    } 
     349               
     350                Quaternion vrpnQuaternion;
     351    uavVrpn->GetQuaternion(vrpnQuaternion);
     352                yawHold=vrpnQuaternion.ToEuler().yaw;
     353
     354                Vector3D vrpnPosition;
     355    uavVrpn->GetPosition(vrpnPosition);
     356    vrpnPosition.To2Dxy(posHold);
    360357
    361358    u_x->Reset();

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

    r122 r135  
    7070        flair::filter::Pid *u_x, *u_y;
    7171
    72         flair::core::Vector2D pos_hold;
    73         float yaw_hold;
     72        flair::core::Vector2D posHold;
     73        float yawHold;
    7474        flair::core::Socket *message;
    7575        flair::core::Time posWait;

  • 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.