Changeset 135 in flair-src for trunk/lib/FlairSensorActuator/src
- Timestamp:
- Feb 13, 2017, 11:38:33 AM (8 years ago)
- Location:
- trunk/lib/FlairSensorActuator/src
- Files:
-
- 4 edited
Legend:
- Unmodified
- Added
- Removed
-
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.