source: flair-src/trunk/lib/FlairFilter/src/JoyReference_impl.cpp@ 7

Last change on this file since 7 was 7, checked in by Sanahuja Guillaume, 6 years ago

filter and meta

File size: 5.6 KB
Line 
1// created: 2012/08/29
2// filename: JoyReference_impl.cpp
3//
4// author: Guillaume Sanahuja
5// Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7// version: $Id: $
8//
9// purpose: generation de consignes a partir joystick
10//
11//
12/*********************************************************************/
13
14#include "JoyReference_impl.h"
15#include "JoyReference.h"
16#include <AhrsData.h>
17#include <Euler.h>
18#include <cvmatrix.h>
19#include <Layout.h>
20#include <GroupBox.h>
21#include <DoubleSpinBox.h>
22#include <SpinBox.h>
23#include <Label.h>
24#include <PushButton.h>
25#include <math.h>
26
27using std::string;
28using namespace flair::core;
29using namespace flair::gui;
30using namespace flair::filter;
31
32JoyReference_impl::JoyReference_impl(JoyReference *inSelf,const LayoutPosition* position,string name): self(inSelf) {
33
34 ahrsData= new AhrsData(self);
35 input=new cvmatrix(self,4,1,floatType,name);
36
37 cvmatrix_descriptor* desc=new cvmatrix_descriptor(4,1);
38 desc->SetElementName(0,0,"z");;
39 desc->SetElementName(1,0,"dz");
40 desc->SetElementName(2,0,"trim_roll");
41 desc->SetElementName(3,0,"trim_pitch");
42 output=new cvmatrix(self,desc,floatType,name);
43
44 reglages_groupbox=new GroupBox(position,name);
45 deb_roll=new DoubleSpinBox(reglages_groupbox->NewRow(),"debattement roll"," deg",-45,45,1,0);
46 deb_pitch=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"debattement pitch"," deg",-45,45,1,0);
47 deb_wz=new DoubleSpinBox(reglages_groupbox->NewRow(),"debattement wz"," deg/s",-180,180,1,0);
48 deb_dz=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"debattement dz"," m/s",-2,2,0.1,1);
49 trim=new DoubleSpinBox(reglages_groupbox->NewRow(),"trim",-1,1,0.01);
50 label_roll=new Label(reglages_groupbox->NewRow(),"trim roll");
51 button_roll=new PushButton(reglages_groupbox->LastRowLastCol(),"reset roll trim");
52 label_pitch=new Label(reglages_groupbox->NewRow(),"trim pitch");
53 button_pitch=new PushButton(reglages_groupbox->LastRowLastCol(),"reset pitch trim");
54
55 z_ref=0;
56 previous_time=0;
57 trim_roll=0;
58 trim_pitch=0;
59
60 label_roll->SetText("trim roll: %.2f",trim_roll);
61 label_pitch->SetText("trim pitch: %.2f",trim_pitch);
62}
63
64JoyReference_impl::~JoyReference_impl(void) {
65}
66
67void JoyReference_impl::SetRollAxis(float value) {
68 input->SetValue(0,0,value);
69}
70
71void JoyReference_impl::SetPitchAxis(float value) {
72 input->SetValue(1,0,value);
73}
74
75void JoyReference_impl::SetYawAxis(float value) {
76 input->SetValue(2,0,value);
77}
78
79void JoyReference_impl::SetAltitudeAxis(float value) {
80 input->SetValue(3,0,value);
81}
82
83void JoyReference_impl::RollTrimUp(void) {
84 trim_roll+=trim->Value();
85 output->SetValue(2,0,trim_roll);
86 label_roll->SetText("trim roll: %.2f",trim_roll);
87}
88
89void JoyReference_impl::RollTrimDown(void) {
90 trim_roll-=trim->Value();
91 output->SetValue(2,0,trim_roll);
92 label_roll->SetText("trim roll: %.2f",trim_roll);
93}
94
95void JoyReference_impl::PitchTrimUp(void) {
96 trim_pitch+=trim->Value();
97 output->SetValue(3,0,trim_pitch);
98 label_pitch->SetText("trim pitch: %.2f",trim_pitch);
99}
100
101void JoyReference_impl::PitchTrimDown(void) {
102 trim_pitch-=trim->Value();
103 output->SetValue(3,0,trim_pitch);
104 label_pitch->SetText("trim pitch: %.2f",trim_pitch);
105}
106
107void JoyReference_impl::SetYawRef(float value) {
108 Euler ref(0,0,value);
109 input->GetMutex();
110 ref.ToQuaternion(q_z);
111 input->ReleaseMutex();
112
113 Update(GetTime());
114}
115
116void JoyReference_impl::SetZRef(float value) {
117 z_ref=value;
118 output->SetValue(0,0,z_ref);
119}
120
121float JoyReference_impl::ZRef(void) const {
122 return output->Value(0,0);
123}
124
125float JoyReference_impl::dZRef(void) const {
126 return output->Value(1,0);
127}
128
129float JoyReference_impl::RollTrim(void) const {
130 return trim_roll;
131}
132
133float JoyReference_impl::PitchTrim(void) const {
134 return trim_pitch;
135}
136
137void JoyReference_impl::Update(Time time) {
138 input->SetDataTime(time);
139 UpdateFrom(input);
140}
141
142void JoyReference_impl::UpdateFrom(const io_data *data) {
143 cvmatrix *input=(cvmatrix*)data;
144
145 if(previous_time==0) previous_time=data->DataTime();//pour la premiere iteration
146 float delta_t=(float)(data->DataTime()-previous_time)/1000000000.;
147 previous_time=data->DataTime();
148
149 if(button_roll->Clicked()==true) {
150 trim_roll=0;
151 output->SetValue(2,0,0);
152 label_roll->SetText("trim roll: %.2f",trim_roll);
153 }
154 if(button_pitch->Clicked()==true) {
155 trim_pitch=0;
156 output->SetValue(3,0,0);
157 label_pitch->SetText("trim pitch: %.2f",trim_pitch);
158 }
159
160 //les box sont en degrés
161 input->GetMutex();
162
163 Vector3D theta_xy(-Euler::ToRadian(input->ValueNoMutex(0,0)*deb_roll->Value()),
164 -Euler::ToRadian(input->ValueNoMutex(1,0)*deb_pitch->Value()),
165 0);
166 Vector3D e_bar=theta_xy;
167 e_bar.Normalize();
168 Quaternion q_xy(cos(theta_xy.GetNorm()/2.0f),
169 e_bar.x*sin(theta_xy.GetNorm()/2.0f),
170 e_bar.y*sin(theta_xy.GetNorm()/2.0f),
171 0);
172 q_xy.Normalize();
173
174 float wz_ref=Euler::ToRadian(input->ValueNoMutex(2,0)*deb_wz->Value());
175 Quaternion w_zd(1,0,0,wz_ref*delta_t/2);
176 w_zd.Normalize();
177 q_z = q_z*w_zd;
178 q_z.Normalize();
179
180 Quaternion q_ref = q_z*q_xy;
181 q_ref.Normalize();
182
183 z_ref+=input->ValueNoMutex(3,0)*deb_dz->Value()*delta_t;
184 float dz_ref=input->ValueNoMutex(3,0)*deb_dz->Value();
185
186 input->ReleaseMutex();
187
188 ahrsData->SetQuaternionAndAngularRates(q_ref,Vector3D(0,0,wz_ref));
189
190 //ouput quaternion for control law
191 output->GetMutex();
192 output->SetValueNoMutex(0,0,z_ref);
193 output->SetValueNoMutex(1,0,dz_ref);
194 output->ReleaseMutex();
195
196 output->SetDataTime(data->DataTime());
197}
198
Note: See TracBrowser for help on using the repository browser.