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