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

Last change on this file since 223 was 223, checked in by Sanahuja Guillaume, 4 years ago

add delta time to io_data

File size: 5.9 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 <Matrix.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,
37 const LayoutPosition *position,
38 string name)
39 : self(inSelf) {
40
41 ahrsData = new AhrsData(self);
42 input = new Matrix(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 Matrix(self, desc, floatType, name);
51 delete desc;
52
53 reglages_groupbox = new GroupBox(position, name);
54 deb_roll = new DoubleSpinBox(reglages_groupbox->NewRow(), "debattement roll",
55 " deg", -45, 45, 1, 0);
56 deb_pitch = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
57 "debattement pitch", " deg", -45, 45, 1, 0);
58 deb_wz = new DoubleSpinBox(reglages_groupbox->NewRow(), "debattement wz",
59 " deg/s", -180, 180, 1, 0);
60 deb_dz = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
61 "debattement dz", " m/s", -2, 2, 0.1, 1);
62 trim = new DoubleSpinBox(reglages_groupbox->NewRow(), "trim", -1, 1, 0.01);
63 label_roll = new Label(reglages_groupbox->NewRow(), "trim roll");
64 button_roll =
65 new PushButton(reglages_groupbox->LastRowLastCol(), "reset roll trim");
66 label_pitch = new Label(reglages_groupbox->NewRow(), "trim pitch");
67 button_pitch =
68 new PushButton(reglages_groupbox->LastRowLastCol(), "reset pitch trim");
69
70 z_ref = 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
78JoyReference_impl::~JoyReference_impl(void) {}
79
80void JoyReference_impl::SetRollAxis(float value) {
81 input->SetValue(0, 0, value);
82}
83
84void JoyReference_impl::SetPitchAxis(float value) {
85 input->SetValue(1, 0, value);
86}
87
88void JoyReference_impl::SetYawAxis(float value) {
89 input->SetValue(2, 0, value);
90}
91
92void JoyReference_impl::SetAltitudeAxis(float value) {
93 input->SetValue(3, 0, value);
94}
95
96void JoyReference_impl::RollTrimUp(void) {
97 trim_roll += trim->Value();
98 output->SetValue(2, 0, trim_roll);
99 label_roll->SetText("trim roll: %.2f", trim_roll);
100}
101
102void JoyReference_impl::RollTrimDown(void) {
103 trim_roll -= trim->Value();
104 output->SetValue(2, 0, trim_roll);
105 label_roll->SetText("trim roll: %.2f", trim_roll);
106}
107
108void JoyReference_impl::PitchTrimUp(void) {
109 trim_pitch += trim->Value();
110 output->SetValue(3, 0, trim_pitch);
111 label_pitch->SetText("trim pitch: %.2f", trim_pitch);
112}
113
114void JoyReference_impl::PitchTrimDown(void) {
115 trim_pitch -= trim->Value();
116 output->SetValue(3, 0, trim_pitch);
117 label_pitch->SetText("trim pitch: %.2f", trim_pitch);
118}
119
120void JoyReference_impl::SetYawRef(float value) {
121 Euler ref(0, 0, value);
122 input->GetMutex();
123 ref.ToQuaternion(q_z);
124 input->ReleaseMutex();
125
126 Update(GetTime());
127}
128
129void JoyReference_impl::SetZRef(float value) {
130 z_ref = value;
131 output->SetValue(0, 0, z_ref);
132}
133
134float JoyReference_impl::ZRef(void) const { return output->Value(0, 0); }
135
136float JoyReference_impl::dZRef(void) const { return output->Value(1, 0); }
137
138float JoyReference_impl::RollTrim(void) const { return trim_roll; }
139
140float JoyReference_impl::PitchTrim(void) const { return trim_pitch; }
141
142void JoyReference_impl::Update(Time time) {
143 input->SetDataTime(time);
144 UpdateFrom(input);
145}
146
147void JoyReference_impl::UpdateFrom(const io_data *data) {
148 const Matrix* input = dynamic_cast<const Matrix*>(data);
149
150 if (!input) {
151 self->Warn("casting %s to Matrix failed\n",data->ObjectName().c_str());
152 return;
153 }
154
155 float delta_t = (float)(data->DataDeltaTime()) / 1000000000.;
156
157 if (button_roll->Clicked() == true) {
158 trim_roll = 0;
159 output->SetValue(2, 0, 0);
160 label_roll->SetText("trim roll: %.2f", trim_roll);
161 }
162 if (button_pitch->Clicked() == true) {
163 trim_pitch = 0;
164 output->SetValue(3, 0, 0);
165 label_pitch->SetText("trim pitch: %.2f", trim_pitch);
166 }
167
168 // les box sont en degrés
169 input->GetMutex();
170
171 Vector3Df theta_xy(
172 -Euler::ToRadian(input->ValueNoMutex(0, 0) * deb_roll->Value()),
173 -Euler::ToRadian(input->ValueNoMutex(1, 0) * deb_pitch->Value()), 0);
174 Vector3Df e_bar = theta_xy;
175 e_bar.Normalize();
176 Quaternion q_xy(cos(theta_xy.GetNorm() / 2.0f),
177 e_bar.x * sin(theta_xy.GetNorm() / 2.0f),
178 e_bar.y * sin(theta_xy.GetNorm() / 2.0f), 0);
179 q_xy.Normalize();
180
181 float wz_ref = Euler::ToRadian(input->ValueNoMutex(2, 0) * deb_wz->Value());
182 Quaternion w_zd(1, 0, 0, wz_ref * delta_t / 2);
183 w_zd.Normalize();
184 q_z = q_z * w_zd;
185 q_z.Normalize();
186
187 Quaternion q_ref = q_z * q_xy;
188 q_ref.Normalize();
189
190 z_ref += input->ValueNoMutex(3, 0) * deb_dz->Value() * delta_t;
191 float dz_ref = input->ValueNoMutex(3, 0) * deb_dz->Value();
192
193 input->ReleaseMutex();
194
195 ahrsData->SetQuaternionAndAngularRates(q_ref, Vector3Df(0, 0, wz_ref));
196
197 // ouput quaternion for control law
198 output->GetMutex();
199 output->SetValueNoMutex(0, 0, z_ref);
200 output->SetValueNoMutex(1, 0, dz_ref);
201 output->ReleaseMutex();
202
203 output->SetDataTime(data->DataTime());
204}
Note: See TracBrowser for help on using the repository browser.