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

Last change on this file since 318 was 318, checked in by Sanahuja Guillaume, 4 years ago
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 MatrixDescriptor *desc = new MatrixDescriptor(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.