Changeset 15 in flair-src for trunk/lib/FlairFilter/src/JoyReference_impl.cpp
- Timestamp:
- Apr 8, 2016, 3:40:57 PM (9 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairFilter/src/JoyReference_impl.cpp
r10 r15 34 34 using namespace flair::filter; 35 35 36 JoyReference_impl::JoyReference_impl(JoyReference *inSelf,const LayoutPosition* position,string name): self(inSelf) { 37 38 ahrsData= new AhrsData(self); 39 input=new cvmatrix(self,4,1,floatType,name); 40 41 cvmatrix_descriptor* desc=new cvmatrix_descriptor(4,1); 42 desc->SetElementName(0,0,"z");; 43 desc->SetElementName(1,0,"dz"); 44 desc->SetElementName(2,0,"trim_roll"); 45 desc->SetElementName(3,0,"trim_pitch"); 46 output=new cvmatrix(self,desc,floatType,name); 47 48 reglages_groupbox=new GroupBox(position,name); 49 deb_roll=new DoubleSpinBox(reglages_groupbox->NewRow(),"debattement roll"," deg",-45,45,1,0); 50 deb_pitch=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"debattement pitch"," deg",-45,45,1,0); 51 deb_wz=new DoubleSpinBox(reglages_groupbox->NewRow(),"debattement wz"," deg/s",-180,180,1,0); 52 deb_dz=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"debattement dz"," m/s",-2,2,0.1,1); 53 trim=new DoubleSpinBox(reglages_groupbox->NewRow(),"trim",-1,1,0.01); 54 label_roll=new Label(reglages_groupbox->NewRow(),"trim roll"); 55 button_roll=new PushButton(reglages_groupbox->LastRowLastCol(),"reset roll trim"); 56 label_pitch=new Label(reglages_groupbox->NewRow(),"trim pitch"); 57 button_pitch=new PushButton(reglages_groupbox->LastRowLastCol(),"reset pitch trim"); 58 59 z_ref=0; 60 previous_time=0; 61 trim_roll=0; 62 trim_pitch=0; 63 64 label_roll->SetText("trim roll: %.2f",trim_roll); 65 label_pitch->SetText("trim pitch: %.2f",trim_pitch); 66 } 67 68 JoyReference_impl::~JoyReference_impl(void) { 69 } 36 JoyReference_impl::JoyReference_impl(JoyReference *inSelf, 37 const LayoutPosition *position, 38 string name) 39 : self(inSelf) { 40 41 ahrsData = new AhrsData(self); 42 input = new cvmatrix(self, 4, 1, floatType, name); 43 44 cvmatrix_descriptor *desc = new cvmatrix_descriptor(4, 1); 45 desc->SetElementName(0, 0, "z"); 46 ; 47 desc->SetElementName(1, 0, "dz"); 48 desc->SetElementName(2, 0, "trim_roll"); 49 desc->SetElementName(3, 0, "trim_pitch"); 50 output = new cvmatrix(self, desc, floatType, name); 51 52 reglages_groupbox = new GroupBox(position, name); 53 deb_roll = new DoubleSpinBox(reglages_groupbox->NewRow(), "debattement roll", 54 " deg", -45, 45, 1, 0); 55 deb_pitch = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), 56 "debattement pitch", " deg", -45, 45, 1, 0); 57 deb_wz = new DoubleSpinBox(reglages_groupbox->NewRow(), "debattement wz", 58 " deg/s", -180, 180, 1, 0); 59 deb_dz = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), 60 "debattement dz", " m/s", -2, 2, 0.1, 1); 61 trim = new DoubleSpinBox(reglages_groupbox->NewRow(), "trim", -1, 1, 0.01); 62 label_roll = new Label(reglages_groupbox->NewRow(), "trim roll"); 63 button_roll = 64 new PushButton(reglages_groupbox->LastRowLastCol(), "reset roll trim"); 65 label_pitch = new Label(reglages_groupbox->NewRow(), "trim pitch"); 66 button_pitch = 67 new PushButton(reglages_groupbox->LastRowLastCol(), "reset pitch trim"); 68 69 z_ref = 0; 70 previous_time = 0; 71 trim_roll = 0; 72 trim_pitch = 0; 73 74 label_roll->SetText("trim roll: %.2f", trim_roll); 75 label_pitch->SetText("trim pitch: %.2f", trim_pitch); 76 } 77 78 JoyReference_impl::~JoyReference_impl(void) {} 70 79 71 80 void JoyReference_impl::SetRollAxis(float value) { 72 input->SetValue(0,0,value);81 input->SetValue(0, 0, value); 73 82 } 74 83 75 84 void JoyReference_impl::SetPitchAxis(float value) { 76 input->SetValue(1,0,value);85 input->SetValue(1, 0, value); 77 86 } 78 87 79 88 void JoyReference_impl::SetYawAxis(float value) { 80 input->SetValue(2,0,value);89 input->SetValue(2, 0, value); 81 90 } 82 91 83 92 void JoyReference_impl::SetAltitudeAxis(float value) { 84 input->SetValue(3,0,value);93 input->SetValue(3, 0, value); 85 94 } 86 95 87 96 void JoyReference_impl::RollTrimUp(void) { 88 trim_roll+=trim->Value();89 output->SetValue(2,0,trim_roll);90 label_roll->SetText("trim roll: %.2f",trim_roll);97 trim_roll += trim->Value(); 98 output->SetValue(2, 0, trim_roll); 99 label_roll->SetText("trim roll: %.2f", trim_roll); 91 100 } 92 101 93 102 void JoyReference_impl::RollTrimDown(void) { 94 trim_roll-=trim->Value();95 output->SetValue(2,0,trim_roll);96 label_roll->SetText("trim roll: %.2f",trim_roll);103 trim_roll -= trim->Value(); 104 output->SetValue(2, 0, trim_roll); 105 label_roll->SetText("trim roll: %.2f", trim_roll); 97 106 } 98 107 99 108 void JoyReference_impl::PitchTrimUp(void) { 100 trim_pitch+=trim->Value();101 output->SetValue(3,0,trim_pitch);102 label_pitch->SetText("trim pitch: %.2f",trim_pitch);109 trim_pitch += trim->Value(); 110 output->SetValue(3, 0, trim_pitch); 111 label_pitch->SetText("trim pitch: %.2f", trim_pitch); 103 112 } 104 113 105 114 void JoyReference_impl::PitchTrimDown(void) { 106 trim_pitch-=trim->Value();107 output->SetValue(3,0,trim_pitch);108 label_pitch->SetText("trim pitch: %.2f",trim_pitch);115 trim_pitch -= trim->Value(); 116 output->SetValue(3, 0, trim_pitch); 117 label_pitch->SetText("trim pitch: %.2f", trim_pitch); 109 118 } 110 119 111 120 void JoyReference_impl::SetYawRef(float value) { 112 Euler ref(0,0,value);113 114 115 116 117 121 Euler ref(0, 0, value); 122 input->GetMutex(); 123 ref.ToQuaternion(q_z); 124 input->ReleaseMutex(); 125 126 Update(GetTime()); 118 127 } 119 128 120 129 void JoyReference_impl::SetZRef(float value) { 121 z_ref=value; 122 output->SetValue(0,0,z_ref); 123 } 124 125 float JoyReference_impl::ZRef(void) const { 126 return output->Value(0,0); 127 } 128 129 float JoyReference_impl::dZRef(void) const { 130 return output->Value(1,0); 131 } 132 133 float JoyReference_impl::RollTrim(void) const { 134 return trim_roll; 135 } 136 137 float JoyReference_impl::PitchTrim(void) const { 138 return trim_pitch; 139 } 130 z_ref = value; 131 output->SetValue(0, 0, z_ref); 132 } 133 134 float JoyReference_impl::ZRef(void) const { return output->Value(0, 0); } 135 136 float JoyReference_impl::dZRef(void) const { return output->Value(1, 0); } 137 138 float JoyReference_impl::RollTrim(void) const { return trim_roll; } 139 140 float JoyReference_impl::PitchTrim(void) const { return trim_pitch; } 140 141 141 142 void JoyReference_impl::Update(Time time) { 142 143 143 input->SetDataTime(time); 144 UpdateFrom(input); 144 145 } 145 146 146 147 void JoyReference_impl::UpdateFrom(const io_data *data) { 147 cvmatrix *input=(cvmatrix*)data; 148 149 if(previous_time==0) previous_time=data->DataTime();//pour la premiere iteration 150 float delta_t=(float)(data->DataTime()-previous_time)/1000000000.; 151 previous_time=data->DataTime(); 152 153 if(button_roll->Clicked()==true) { 154 trim_roll=0; 155 output->SetValue(2,0,0); 156 label_roll->SetText("trim roll: %.2f",trim_roll); 157 } 158 if(button_pitch->Clicked()==true) { 159 trim_pitch=0; 160 output->SetValue(3,0,0); 161 label_pitch->SetText("trim pitch: %.2f",trim_pitch); 162 } 163 164 //les box sont en degrés 165 input->GetMutex(); 166 167 Vector3D theta_xy(-Euler::ToRadian(input->ValueNoMutex(0,0)*deb_roll->Value()), 168 -Euler::ToRadian(input->ValueNoMutex(1,0)*deb_pitch->Value()), 169 0); 170 Vector3D e_bar=theta_xy; 171 e_bar.Normalize(); 172 Quaternion q_xy(cos(theta_xy.GetNorm()/2.0f), 173 e_bar.x*sin(theta_xy.GetNorm()/2.0f), 174 e_bar.y*sin(theta_xy.GetNorm()/2.0f), 175 0); 176 q_xy.Normalize(); 177 178 float wz_ref=Euler::ToRadian(input->ValueNoMutex(2,0)*deb_wz->Value()); 179 Quaternion w_zd(1,0,0,wz_ref*delta_t/2); 180 w_zd.Normalize(); 181 q_z = q_z*w_zd; 182 q_z.Normalize(); 183 184 Quaternion q_ref = q_z*q_xy; 185 q_ref.Normalize(); 186 187 z_ref+=input->ValueNoMutex(3,0)*deb_dz->Value()*delta_t; 188 float dz_ref=input->ValueNoMutex(3,0)*deb_dz->Value(); 189 190 input->ReleaseMutex(); 191 192 ahrsData->SetQuaternionAndAngularRates(q_ref,Vector3D(0,0,wz_ref)); 193 194 //ouput quaternion for control law 195 output->GetMutex(); 196 output->SetValueNoMutex(0,0,z_ref); 197 output->SetValueNoMutex(1,0,dz_ref); 198 output->ReleaseMutex(); 199 200 output->SetDataTime(data->DataTime()); 201 } 202 148 cvmatrix *input = (cvmatrix *)data; 149 150 if (previous_time == 0) 151 previous_time = data->DataTime(); // pour la premiere iteration 152 float delta_t = (float)(data->DataTime() - previous_time) / 1000000000.; 153 previous_time = data->DataTime(); 154 155 if (button_roll->Clicked() == true) { 156 trim_roll = 0; 157 output->SetValue(2, 0, 0); 158 label_roll->SetText("trim roll: %.2f", trim_roll); 159 } 160 if (button_pitch->Clicked() == true) { 161 trim_pitch = 0; 162 output->SetValue(3, 0, 0); 163 label_pitch->SetText("trim pitch: %.2f", trim_pitch); 164 } 165 166 // les box sont en degrés 167 input->GetMutex(); 168 169 Vector3D theta_xy( 170 -Euler::ToRadian(input->ValueNoMutex(0, 0) * deb_roll->Value()), 171 -Euler::ToRadian(input->ValueNoMutex(1, 0) * deb_pitch->Value()), 0); 172 Vector3D e_bar = theta_xy; 173 e_bar.Normalize(); 174 Quaternion q_xy(cos(theta_xy.GetNorm() / 2.0f), 175 e_bar.x * sin(theta_xy.GetNorm() / 2.0f), 176 e_bar.y * sin(theta_xy.GetNorm() / 2.0f), 0); 177 q_xy.Normalize(); 178 179 float wz_ref = Euler::ToRadian(input->ValueNoMutex(2, 0) * deb_wz->Value()); 180 Quaternion w_zd(1, 0, 0, wz_ref * delta_t / 2); 181 w_zd.Normalize(); 182 q_z = q_z * w_zd; 183 q_z.Normalize(); 184 185 Quaternion q_ref = q_z * q_xy; 186 q_ref.Normalize(); 187 188 z_ref += input->ValueNoMutex(3, 0) * deb_dz->Value() * delta_t; 189 float dz_ref = input->ValueNoMutex(3, 0) * deb_dz->Value(); 190 191 input->ReleaseMutex(); 192 193 ahrsData->SetQuaternionAndAngularRates(q_ref, Vector3D(0, 0, wz_ref)); 194 195 // ouput quaternion for control law 196 output->GetMutex(); 197 output->SetValueNoMutex(0, 0, z_ref); 198 output->SetValueNoMutex(1, 0, dz_ref); 199 output->ReleaseMutex(); 200 201 output->SetDataTime(data->DataTime()); 202 }
Note:
See TracChangeset
for help on using the changeset viewer.