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


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/demos/SimpleFleet/uav/src
Files:
2 edited

Legend:

Unmodified
Added
Removed
  • 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;
Note: See TracChangeset for help on using the changeset viewer.