source: flair-src/trunk/lib/FlairFilter/src/X4X8Multiplex_impl.cpp @ 106

Last change on this file since 106 was 106, checked in by Sanahuja Guillaume, 5 years ago

modifs

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:    2014/04/10
6//  filename:   X4X8Multiplex_impl.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    Class defining X4 and X8 multiplexing
14//
15//
16/*********************************************************************/
17
18#include "X4X8Multiplex_impl.h"
19#include "X4X8Multiplex.h"
20#include <cvmatrix.h>
21#include <Tab.h>
22#include <GridLayout.h>
23#include <GroupBox.h>
24#include <ComboBox.h>
25#include <DataPlot1D.h>
26#include <math.h>
27
28#define MAX_MOTORS 8
29
30using std::string;
31using namespace flair::core;
32using namespace flair::gui;
33using namespace flair::filter;
34
35X4X8Multiplex_impl::X4X8Multiplex_impl(flair::filter::X4X8Multiplex *self,
36                                       int nb_mot) {
37  this->nb_mot = nb_mot;
38  this->self = self;
39
40  if (nb_mot == 4) {
41    GroupBox *groupbox =
42        new GroupBox(self->GetLayout()->NewRow(), "x4 multiplex");
43    pas = new ComboBox(groupbox->NewRow(), "front left blade pitch:");
44  } else {
45    GroupBox *groupbox =
46        new GroupBox(self->GetLayout()->NewRow(), "x8 multiplex");
47    pas = new ComboBox(groupbox->NewRow(), "top front left blade pitch:");
48  }
49  pas->AddItem("counter clockwise");
50  pas->AddItem("clockwise");
51
52  cvmatrix_descriptor *desc = new cvmatrix_descriptor(nb_mot, 1);
53  for (int i = 0; i < nb_mot; i++) {
54    desc->SetElementName(i, 0, MotorName(i));
55  }
56
57  output = new cvmatrix(self, desc, floatType);
58
59  self->AddDataToLog(output);
60}
61
62X4X8Multiplex_impl::~X4X8Multiplex_impl(void) {}
63
64void X4X8Multiplex_impl::UseDefaultPlot(void) {
65  Tab *plot_tab = new Tab(self->GetTabWidget(), "Values");
66  plots[0] = new DataPlot1D(plot_tab->NewRow(), "front left", 0, 1);
67  plots[1] = new DataPlot1D(plot_tab->LastRowLastCol(), "front right", 0, 1);
68  plots[2] = new DataPlot1D(plot_tab->NewRow(), "rear left", 0, 1);
69  plots[3] = new DataPlot1D(plot_tab->LastRowLastCol(), "rear right", 0, 1);
70
71  for (int i = 0; i < 4; i++)
72    plots[i]->AddCurve(output->Element(i));
73
74  if (nb_mot == 8) {
75    for (int i = 0; i < 4; i++)
76      plots[i]->AddCurve(output->Element(i + 4), DataPlot::Blue);
77  }
78}
79
80void X4X8Multiplex_impl::UpdateFrom(const io_data *data) {
81  float u_roll, u_pitch, u_yaw, u_thrust;
82  float trim_roll, trim_pitch, trim_yaw;
83  float value[MAX_MOTORS];
84
85  cvmatrix *input = (cvmatrix *)data;
86
87  // on prend une fois pour toute le mutex et on fait des accès directs
88  input->GetMutex();
89
90  u_roll = input->ValueNoMutex(0, 0);
91  u_pitch = input->ValueNoMutex(1, 0);
92  u_yaw = input->ValueNoMutex(2, 0);
93  u_thrust = input->ValueNoMutex(3, 0);
94  trim_roll = input->ValueNoMutex(4, 0);
95  trim_pitch = input->ValueNoMutex(5, 0);
96  trim_yaw = input->ValueNoMutex(6, 0);
97
98  input->ReleaseMutex();
99
100  if (pas->CurrentIndex() == 1) {
101    trim_yaw = -trim_yaw;
102    u_yaw = -u_yaw;
103  }
104
105  if (nb_mot == 2) {
106    //(top) front left
107    value[self->MultiplexValue(X4X8Multiplex::TopFrontLeft)] = Set(
108        trim_pitch + trim_roll + trim_yaw, u_thrust + u_pitch + u_roll + u_yaw);
109
110    //(top) front right
111    value[self->MultiplexValue(X4X8Multiplex::TopFrontRight)] = Set(
112        trim_pitch - trim_roll - trim_yaw, u_thrust + u_pitch - u_roll - u_yaw);
113  }
114  if (nb_mot == 4 || nb_mot == 8) {
115    //(top) front left
116    value[self->MultiplexValue(X4X8Multiplex::TopFrontLeft)] = Set(
117        trim_pitch + trim_roll + trim_yaw, u_thrust + u_pitch + u_roll + u_yaw);
118
119    //(top) front right
120    value[self->MultiplexValue(X4X8Multiplex::TopFrontRight)] = Set(
121        trim_pitch - trim_roll - trim_yaw, u_thrust + u_pitch - u_roll - u_yaw);
122
123    //(top) rear left
124    value[self->MultiplexValue(X4X8Multiplex::TopRearLeft)] =
125        Set(-trim_pitch + trim_roll - trim_yaw,
126            u_thrust - u_pitch + u_roll - u_yaw);
127
128    //(top) rear right
129    value[self->MultiplexValue(X4X8Multiplex::TopRearRight)] =
130        Set(-trim_pitch - trim_roll + trim_yaw,
131            u_thrust - u_pitch - u_roll + u_yaw);
132  }
133
134  if (nb_mot == 8) {
135    // bottom front left
136    value[self->MultiplexValue(X4X8Multiplex::BottomFrontLeft)] = Set(
137        trim_pitch + trim_roll - trim_yaw, u_thrust + u_pitch + u_roll - u_yaw);
138
139    // bottom front right
140    value[self->MultiplexValue(X4X8Multiplex::BottomFrontRight)] = Set(
141        trim_pitch - trim_roll + trim_yaw, u_thrust + u_pitch - u_roll + u_yaw);
142
143    // bottom rear left
144    value[self->MultiplexValue(X4X8Multiplex::BottomRearLeft)] =
145        Set(-trim_pitch + trim_roll + trim_yaw,
146            u_thrust - u_pitch + u_roll + u_yaw);
147
148    // bottom rear right
149    value[self->MultiplexValue(X4X8Multiplex::BottomRearRight)] =
150        Set(-trim_pitch - trim_roll - trim_yaw,
151            u_thrust - u_pitch - u_roll - u_yaw);
152  }
153
154  // on prend une fois pour toute le mutex et on fait des accès directs
155  output->GetMutex();
156  for (int i = 0; i < nb_mot; i++)
157    output->SetValueNoMutex(i, 0, value[i]);
158  output->ReleaseMutex();
159
160  output->SetDataTime(data->DataTime());
161
162  self->ProcessUpdate(output);
163}
164
165float X4X8Multiplex_impl::Set(float trim, float u) {
166  float value = trim;
167
168  if (u > 0) {
169    value += sqrtf(u);
170  }
171
172  return value;
173}
174
175string X4X8Multiplex_impl::MotorName(int index) {
176  switch (nb_mot) {
177  case 4: {
178    switch (index) {
179    case 0:
180      return "front left";
181    case 1:
182      return "front rigth";
183    case 2:
184      return "rear left";
185    case 3:
186      return "rear rigth";
187    default:
188      return "unammed motor";
189    }
190  }
191  case 8: {
192    switch (index) {
193    case 0:
194      return "top front left";
195    case 1:
196      return "top front rigth";
197    case 2:
198      return "top rear left";
199    case 3:
200      return "top rear rigth";
201    case 4:
202      return "bottom front left";
203    case 5:
204      return "bottom front rigth";
205    case 6:
206      return "bottom rear left";
207    case 7:
208      return "bottom rear rigth";
209    default:
210      return "unammed motor";
211    }
212  }
213  default: { return "unammed motor"; }
214  }
215}
Note: See TracBrowser for help on using the repository browser.