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

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

matrix

File size: 6.1 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 <Matrix.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 Matrix(self, desc, floatType);
58  delete desc;
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  const Matrix* input = dynamic_cast<const Matrix*>(data);
86 
87  if (!input) {
88      self->Warn("casting %s to Matrix failed\n",data->ObjectName().c_str());
89      return;
90  }
91
92  // on prend une fois pour toute le mutex et on fait des accès directs
93  input->GetMutex();
94
95  u_roll = input->ValueNoMutex(0, 0);
96  u_pitch = input->ValueNoMutex(1, 0);
97  u_yaw = input->ValueNoMutex(2, 0);
98  u_thrust = input->ValueNoMutex(3, 0);
99  trim_roll = input->ValueNoMutex(4, 0);
100  trim_pitch = input->ValueNoMutex(5, 0);
101  trim_yaw = input->ValueNoMutex(6, 0);
102
103  input->ReleaseMutex();
104
105  if (pas->CurrentIndex() == 1) {
106    trim_yaw = -trim_yaw;
107    u_yaw = -u_yaw;
108  }
109
110  if (nb_mot == 2) {
111    //(top) front left
112    value[self->MultiplexValue(X4X8Multiplex::TopFrontLeft)] = Set(
113        trim_pitch + trim_roll + trim_yaw, u_thrust + u_pitch + u_roll + u_yaw);
114
115    //(top) front right
116    value[self->MultiplexValue(X4X8Multiplex::TopFrontRight)] = Set(
117        trim_pitch - trim_roll - trim_yaw, u_thrust + u_pitch - u_roll - u_yaw);
118  }
119  if (nb_mot == 4 || nb_mot == 8) {
120    //(top) front left
121    value[self->MultiplexValue(X4X8Multiplex::TopFrontLeft)] = Set(
122        trim_pitch + trim_roll + trim_yaw, u_thrust + u_pitch + u_roll + u_yaw);
123
124    //(top) front right
125    value[self->MultiplexValue(X4X8Multiplex::TopFrontRight)] = Set(
126        trim_pitch - trim_roll - trim_yaw, u_thrust + u_pitch - u_roll - u_yaw);
127
128    //(top) rear left
129    value[self->MultiplexValue(X4X8Multiplex::TopRearLeft)] =
130        Set(-trim_pitch + trim_roll - trim_yaw,
131            u_thrust - u_pitch + u_roll - u_yaw);
132
133    //(top) rear right
134    value[self->MultiplexValue(X4X8Multiplex::TopRearRight)] =
135        Set(-trim_pitch - trim_roll + trim_yaw,
136            u_thrust - u_pitch - u_roll + u_yaw);
137  }
138
139  if (nb_mot == 8) {
140    // bottom front left
141    value[self->MultiplexValue(X4X8Multiplex::BottomFrontLeft)] = Set(
142        trim_pitch + trim_roll - trim_yaw, u_thrust + u_pitch + u_roll - u_yaw);
143
144    // bottom front right
145    value[self->MultiplexValue(X4X8Multiplex::BottomFrontRight)] = Set(
146        trim_pitch - trim_roll + trim_yaw, u_thrust + u_pitch - u_roll + u_yaw);
147
148    // bottom rear left
149    value[self->MultiplexValue(X4X8Multiplex::BottomRearLeft)] =
150        Set(-trim_pitch + trim_roll + trim_yaw,
151            u_thrust - u_pitch + u_roll + u_yaw);
152
153    // bottom rear right
154    value[self->MultiplexValue(X4X8Multiplex::BottomRearRight)] =
155        Set(-trim_pitch - trim_roll - trim_yaw,
156            u_thrust - u_pitch - u_roll - u_yaw);
157  }
158
159  // on prend une fois pour toute le mutex et on fait des accès directs
160  output->GetMutex();
161  for (int i = 0; i < nb_mot; i++)
162    output->SetValueNoMutex(i, 0, value[i]);
163  output->ReleaseMutex();
164
165  output->SetDataTime(data->DataTime());
166
167  self->ProcessUpdate(output);
168}
169
170float X4X8Multiplex_impl::Set(float trim, float u) {
171  float value = trim;
172
173  if (u > 0) {
174    value += sqrtf(u);
175  }
176
177  return value;
178}
179
180string X4X8Multiplex_impl::MotorName(int index) {
181  switch (nb_mot) {
182  case 4: {
183    switch (index) {
184    case 0:
185      return "front left";
186    case 1:
187      return "front rigth";
188    case 2:
189      return "rear left";
190    case 3:
191      return "rear rigth";
192    default:
193      return "unammed motor";
194    }
195  }
196  case 8: {
197    switch (index) {
198    case 0:
199      return "top front left";
200    case 1:
201      return "top front rigth";
202    case 2:
203      return "top rear left";
204    case 3:
205      return "top rear rigth";
206    case 4:
207      return "bottom front left";
208    case 5:
209      return "bottom front rigth";
210    case 6:
211      return "bottom rear left";
212    case 7:
213      return "bottom rear rigth";
214    default:
215      return "unammed motor";
216    }
217  }
218  default: { return "unammed motor"; }
219  }
220}
Note: See TracBrowser for help on using the repository browser.