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

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

lic

File size: 5.7 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5// created: 2012/08/29
6// filename: JoyReference_impl.cpp
7//
8// author: Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: generation de consignes a partir joystick
14//
15//
16/*********************************************************************/
17
18#include "JoyReference_impl.h"
19#include "JoyReference.h"
20#include <AhrsData.h>
21#include <Euler.h>
22#include <cvmatrix.h>
23#include <Layout.h>
24#include <GroupBox.h>
25#include <DoubleSpinBox.h>
26#include <SpinBox.h>
27#include <Label.h>
28#include <PushButton.h>
29#include <math.h>
30
31using std::string;
32using namespace flair::core;
33using namespace flair::gui;
34using namespace flair::filter;
35
36JoyReference_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
68JoyReference_impl::~JoyReference_impl(void) {
69}
70
71void JoyReference_impl::SetRollAxis(float value) {
72 input->SetValue(0,0,value);
73}
74
75void JoyReference_impl::SetPitchAxis(float value) {
76 input->SetValue(1,0,value);
77}
78
79void JoyReference_impl::SetYawAxis(float value) {
80 input->SetValue(2,0,value);
81}
82
83void JoyReference_impl::SetAltitudeAxis(float value) {
84 input->SetValue(3,0,value);
85}
86
87void 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);
91}
92
93void 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);
97}
98
99void 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);
103}
104
105void 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);
109}
110
111void JoyReference_impl::SetYawRef(float value) {
112 Euler ref(0,0,value);
113 input->GetMutex();
114 ref.ToQuaternion(q_z);
115 input->ReleaseMutex();
116
117 Update(GetTime());
118}
119
120void JoyReference_impl::SetZRef(float value) {
121 z_ref=value;
122 output->SetValue(0,0,z_ref);
123}
124
125float JoyReference_impl::ZRef(void) const {
126 return output->Value(0,0);
127}
128
129float JoyReference_impl::dZRef(void) const {
130 return output->Value(1,0);
131}
132
133float JoyReference_impl::RollTrim(void) const {
134 return trim_roll;
135}
136
137float JoyReference_impl::PitchTrim(void) const {
138 return trim_pitch;
139}
140
141void JoyReference_impl::Update(Time time) {
142 input->SetDataTime(time);
143 UpdateFrom(input);
144}
145
146void 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
Note: See TracBrowser for help on using the repository browser.