Changeset 135 in flairsrc for trunk/demos/SimpleFleet
 Timestamp:
 02/13/17 11:38:33 (6 years ago)
 Location:
 trunk/demos/SimpleFleet/uav/src
 Files:

 2 edited
Legend:
 Unmodified
 Added
 Removed

trunk/demos/SimpleFleet/uav/src/SimpleFleet.cpp
r129 r135 106 106 const AhrsData *SimpleFleet::GetOrientation(void) const { 107 107 //get yaw from vrpn 108 Euler vrpnEuler;109 uavVrpn>Get Euler(vrpnEuler);108 Quaternion vrpnQuaternion; 109 uavVrpn>GetQuaternion(vrpnQuaternion); 110 110 111 111 //get roll, pitch and w from imu … … 115 115 116 116 Euler ahrsEuler=ahrsQuaternion.ToEuler(); 117 ahrsEuler.yaw=vrpn Euler.yaw;117 ahrsEuler.yaw=vrpnQuaternion.ToEuler().yaw; 118 118 Quaternion mixQuaternion=ahrsEuler.ToQuaternion(); 119 119 … … 158 158 Vector3D uav_pos,uav_vel; // in VRPN coordinate system 159 159 Vector2D uav_2Dpos,uav_2Dvel; // in VRPN coordinate system 160 Euler vrpn_euler; // in VRPN coordinate system161 160 162 161 uavVrpn>GetPosition(uav_pos); 163 162 uavVrpn>GetSpeed(uav_vel); 164 uavVrpn>GetEuler(vrpn_euler);165 163 166 164 uav_pos.To2Dxy(uav_2Dpos); … … 169 167 if (behaviourMode==BehaviourMode_t::PositionHold1  behaviourMode==BehaviourMode_t::PositionHold2 170 168  behaviourMode==BehaviourMode_t::PositionHold3  behaviourMode==BehaviourMode_t::PositionHold4) { 171 pos_error=uav_2Dpospos _hold;169 pos_error=uav_2DposposHold; 172 170 vel_error=uav_2Dvel; 173 yaw_ref=yaw _hold;171 yaw_ref=yawHold; 174 172 } else { //Circle 175 173 Vector2D circle_pos,circle_vel; … … 222 220 VrpnPositionHold(); 223 221 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(); 226 224 } else { 227 pos _hold.y+=yDisplacement>Value();225 posHold.y+=yDisplacement>Value(); 228 226 } 229 227 posWait=GetTime(); … … 232 230 if (behaviourMode==BehaviourMode_t::PositionHold2 && GetTime()>(posWait+3*(Time)1000000000)) { 233 231 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(); 236 234 } else { 237 pos _hold.y=yDisplacement>Value();235 posHold.y=yDisplacement>Value(); 238 236 } 239 237 posWait=GetTime(); … … 343 341 344 342 void SimpleFleet::VrpnPositionHold(void) { 345 Euler vrpn_euler; 346 Vector3D vrpn_pos; 347 348 if (SetOrientationMode(OrientationMode_t::Custom)) { 343 if (SetOrientationMode(OrientationMode_t::Custom)) { 349 344 Thread::Info("Demo flotte: holding position\n"); 350 345 } else { 351 346 Thread::Info("Demo flotte: could not hold position\n"); 352 347 //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); 360 357 361 358 u_x>Reset(); 
trunk/demos/SimpleFleet/uav/src/SimpleFleet.h
r122 r135 70 70 flair::filter::Pid *u_x, *u_y; 71 71 72 flair::core::Vector2D pos _hold;73 float yaw _hold;72 flair::core::Vector2D posHold; 73 float yawHold; 74 74 flair::core::Socket *message; 75 75 flair::core::Time posWait;
Note:
See TracChangeset
for help on using the changeset viewer.