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

Last change on this file was 336, checked in by Sanahuja Guillaume, 2 years ago

use float

File size: 5.9 KB
RevLine 
[10]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[10]4// %flair:license}
[7]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>
[214]22#include <Matrix.h>
[7]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
[15]36JoyReference_impl::JoyReference_impl(JoyReference *inSelf,
37                                     const LayoutPosition *position,
38                                     string name)
39    : self(inSelf) {
[7]40
[15]41  ahrsData = new AhrsData(self);
[214]42  input = new Matrix(self, 4, 1, floatType, name);
[7]43
[318]44  MatrixDescriptor *desc = new MatrixDescriptor(4, 1);
[15]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");
[214]50  output = new Matrix(self, desc, floatType, name);
[148]51  delete desc;
[7]52
[15]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");
[7]69
[15]70  z_ref = 0;
71  trim_roll = 0;
72  trim_pitch = 0;
[7]73
[15]74  label_roll->SetText("trim roll: %.2f", trim_roll);
75  label_pitch->SetText("trim pitch: %.2f", trim_pitch);
[7]76}
77
[15]78JoyReference_impl::~JoyReference_impl(void) {}
[7]79
80void JoyReference_impl::SetRollAxis(float value) {
[15]81  input->SetValue(0, 0, value);
[7]82}
83
84void JoyReference_impl::SetPitchAxis(float value) {
[15]85  input->SetValue(1, 0, value);
[7]86}
87
88void JoyReference_impl::SetYawAxis(float value) {
[15]89  input->SetValue(2, 0, value);
[7]90}
91
92void JoyReference_impl::SetAltitudeAxis(float value) {
[15]93  input->SetValue(3, 0, value);
[7]94}
95
96void JoyReference_impl::RollTrimUp(void) {
[15]97  trim_roll += trim->Value();
98  output->SetValue(2, 0, trim_roll);
99  label_roll->SetText("trim roll: %.2f", trim_roll);
[7]100}
101
102void JoyReference_impl::RollTrimDown(void) {
[15]103  trim_roll -= trim->Value();
104  output->SetValue(2, 0, trim_roll);
105  label_roll->SetText("trim roll: %.2f", trim_roll);
[7]106}
107
108void JoyReference_impl::PitchTrimUp(void) {
[15]109  trim_pitch += trim->Value();
110  output->SetValue(3, 0, trim_pitch);
111  label_pitch->SetText("trim pitch: %.2f", trim_pitch);
[7]112}
113
114void JoyReference_impl::PitchTrimDown(void) {
[15]115  trim_pitch -= trim->Value();
116  output->SetValue(3, 0, trim_pitch);
117  label_pitch->SetText("trim pitch: %.2f", trim_pitch);
[7]118}
119
120void JoyReference_impl::SetYawRef(float value) {
[15]121  Euler ref(0, 0, value);
122  input->GetMutex();
123  ref.ToQuaternion(q_z);
124  input->ReleaseMutex();
[7]125
[15]126  Update(GetTime());
[7]127}
128
129void JoyReference_impl::SetZRef(float value) {
[15]130  z_ref = value;
131  output->SetValue(0, 0, z_ref);
[7]132}
133
[15]134float JoyReference_impl::ZRef(void) const { return output->Value(0, 0); }
[7]135
[15]136float JoyReference_impl::dZRef(void) const { return output->Value(1, 0); }
[7]137
[15]138float JoyReference_impl::RollTrim(void) const { return trim_roll; }
[7]139
[15]140float JoyReference_impl::PitchTrim(void) const { return trim_pitch; }
[7]141
142void JoyReference_impl::Update(Time time) {
[15]143  input->SetDataTime(time);
144  UpdateFrom(input);
[7]145}
146
147void JoyReference_impl::UpdateFrom(const io_data *data) {
[214]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  }
[7]154
[223]155  float delta_t = (float)(data->DataDeltaTime()) / 1000000000.;
[7]156
[15]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  }
[7]167
[15]168  // les box sont en degrés
169  input->GetMutex();
[7]170
[167]171  Vector3Df theta_xy(
[15]172      -Euler::ToRadian(input->ValueNoMutex(0, 0) * deb_roll->Value()),
173      -Euler::ToRadian(input->ValueNoMutex(1, 0) * deb_pitch->Value()), 0);
[167]174  Vector3Df e_bar = theta_xy;
[15]175  e_bar.Normalize();
[336]176  Quaternion q_xy(cosf(theta_xy.GetNorm() / 2.0f),
177                  e_bar.x * sinf(theta_xy.GetNorm() / 2.0f),
178                  e_bar.y * sinf(theta_xy.GetNorm() / 2.0f), 0);
[15]179  q_xy.Normalize();
[7]180
[15]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();
[7]186
[15]187  Quaternion q_ref = q_z * q_xy;
188  q_ref.Normalize();
[7]189
[15]190  z_ref += input->ValueNoMutex(3, 0) * deb_dz->Value() * delta_t;
191  float dz_ref = input->ValueNoMutex(3, 0) * deb_dz->Value();
[7]192
[15]193  input->ReleaseMutex();
[7]194
[167]195  ahrsData->SetQuaternionAndAngularRates(q_ref, Vector3Df(0, 0, wz_ref));
[7]196
[15]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();
[7]202
[15]203  output->SetDataTime(data->DataTime());
[7]204}
Note: See TracBrowser for help on using the repository browser.