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, 5 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.