Changeset 135 in flair-src
- Timestamp:
- Feb 13, 2017, 11:38:33 AM (8 years ago)
- Location:
- trunk
- Files:
-
- 9 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/ReleaseNotes
r133 r135 10 10 - Uav class is now a singleton 11 11 - VrpnClient class is now a singleton 12 - VrpnObject no longer outputs Euler (ony Quaternion): warning, output matrix has changed! 12 13 13 14 ----------------------------------------------------------- -
trunk/demos/CircleFollower/uav/src/CircleFollower.cpp
r129 r135 84 84 const AhrsData *CircleFollower::GetOrientation(void) const { 85 85 //get yaw from vrpn 86 Euler vrpnEuler;87 uavVrpn->Get Euler(vrpnEuler);86 Quaternion vrpnQuaternion; 87 uavVrpn->GetQuaternion(vrpnQuaternion); 88 88 89 89 //get roll, pitch and w from imu … … 93 93 94 94 Euler ahrsEuler=ahrsQuaternion.ToEuler(); 95 ahrsEuler.yaw=vrpn Euler.yaw;95 ahrsEuler.yaw=vrpnQuaternion.ToEuler().yaw; 96 96 Quaternion mixQuaternion=ahrsEuler.ToQuaternion(); 97 97 … … 261 261 262 262 void 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); 271 270 272 271 uX->Reset(); -
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_2Dpos-pos _hold;169 pos_error=uav_2Dpos-posHold; 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; -
trunk/lib/FlairMeta/src/MetaVrpnObject.cpp
r122 r135 51 51 52 52 void 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++) { 55 55 desc->SetElementName(i, 0, Output()->Name(i, 0)); 56 56 } 57 57 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++) { 59 59 prev_value->SetValue(i, 0, 0); 60 60 } … … 63 63 name + " Passe bas", prev_value); 64 64 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++) { 67 67 desc->SetElementName(i, 0, "d" + Output()->Name(i, 0)); 68 68 } 69 69 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++) { 71 71 prev_value->SetValue(i, 0, 0); 72 72 } … … 76 76 77 77 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)); 79 79 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)); 81 81 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)); 83 83 84 84 plot_tab = new Tab(GetVrpnClient()->GetTabWidget(), "Mesures (xy) " + name); 85 85 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)); 87 87 } 88 88 … … 98 98 99 99 void 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); 103 103 } 104 104 -
trunk/lib/FlairSensorActuator/src/VrpnObject.cpp
r122 r135 67 67 } 68 68 69 void VrpnObject::GetEuler(Euler &euler) const { pimpl_->GetEuler(euler); }70 71 69 void VrpnObject::GetQuaternion(Quaternion &quaternion) const { 72 70 pimpl_->GetQuaternion(quaternion); -
trunk/lib/FlairSensorActuator/src/VrpnObject.h
r122 r135 22 22 class cvmatrix; 23 23 class Vector3D; 24 class Euler;25 24 class Quaternion; 26 25 } … … 101 100 102 101 /*! 103 * \brief Get Euler angles104 *105 * \param euler output datas106 */107 void GetEuler(core::Euler &euler) const;108 109 /*!110 102 * \brief Get Quaternion 111 103 * … … 124 116 * 125 117 * 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 132 125 * 133 * \return Output m etrix126 * \return Output matrix 134 127 */ 135 128 core::cvmatrix *Output(void) const; … … 138 131 * \brief State matrix 139 132 * 133 * Used for plotting on ground station. \n 140 134 * Matrix is of type float and as follows: \n 141 135 * (0,0) roll (deg) \n … … 143 137 * (2,0) yaw (deg) \n 144 138 * 145 * \return State m etrix139 * \return State matrix 146 140 */ 147 141 core::cvmatrix *State(void) const; -
trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp
r122 r135 67 67 68 68 // 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"); 76 77 output = new cvmatrix(self, desc, floatType); 77 78 … … 85 86 plot_tab = new Tab(tab, "Mesures " + name); 86 87 x_plot = new DataPlot1D(plot_tab->NewRow(), "x", -10, 10); 87 x_plot->AddCurve(output->Element( 3));88 x_plot->AddCurve(output->Element(4)); 88 89 y_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "y", -10, 10); 89 y_plot->AddCurve(output->Element( 4));90 y_plot->AddCurve(output->Element(5)); 90 91 z_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "z", -2, 0); 91 z_plot->AddCurve(output->Element( 5));92 z_plot->AddCurve(output->Element(6)); 92 93 } 93 94 … … 122 123 } 123 124 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 132 125 void VrpnObject_impl::GetQuaternion(Quaternion &quaternion) { 133 126 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); 138 131 output->ReleaseMutex(); 139 132 } … … 141 134 void VrpnObject_impl::GetPosition(Vector3D &point) { 142 135 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); 146 139 output->ReleaseMutex(); 147 140 } … … 172 165 // warning: t.quat is defined as (qx,qy,qz,qw), which is different from 173 166 // 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]); 178 168 Vector3D pos((float)t.pos[0], (float)t.pos[1], (float)t.pos[2]); 179 169 180 170 // on effectue les rotation 181 171 caller->parent->pimpl_->ComputeRotations(pos); 182 caller->parent->pimpl_->ComputeRotations( caller->quaternion);172 caller->parent->pimpl_->ComputeRotations(quaternion); 183 173 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); 192 181 193 182 caller->output->SetDataTime(time); 194 183 caller->output->ReleaseMutex(); 195 184 185 Euler euler=quaternion.ToEuler(); 196 186 caller->state->GetMutex(); 197 187 caller->state->SetValueNoMutex(0, 0, Euler::ToDegree(euler.roll)); -
trunk/lib/FlairSensorActuator/src/unexported/VrpnObject_impl.h
r122 r135 29 29 class cvmatrix; 30 30 class Vector3D; 31 class Euler;32 31 } 33 32 namespace gui { … … 52 51 53 52 void mainloop(void); 54 void GetEuler(flair::core::Euler &euler);55 53 void GetQuaternion(flair::core::Quaternion &quaternion); 56 54 void GetPosition(flair::core::Vector3D &point); … … 69 67 const flair::sensor::VrpnClient *parent; 70 68 vrpn_Tracker_Remote *tracker; 71 flair::core::Quaternion quaternion; // todo: quaternion should be included in72 // the output to replace euler angles73 69 void Update(void); 74 70 };
Note:
See TracChangeset
for help on using the changeset viewer.