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

Last change on this file since 10 was 10, checked in by Sanahuja Guillaume, 6 years ago

lic

File size: 6.4 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,int nb_mot)
36{
37    this->nb_mot=nb_mot;
38    this->self=self;
39
40    if(nb_mot==4)
41    {
42        GroupBox *groupbox=new GroupBox(self->GetLayout()->NewRow(),"x4 multiplex");
43        pas=new ComboBox(groupbox->NewRow(),"front left blade pitch:");
44    }
45    else
46    {
47        GroupBox *groupbox=new GroupBox(self->GetLayout()->NewRow(),"x8 multiplex");
48        pas=new ComboBox(groupbox->NewRow(),"top front left blade pitch:");
49    }
50    pas->AddItem("normal");
51    pas->AddItem("inverted");
52
53    cvmatrix_descriptor* desc=new cvmatrix_descriptor(nb_mot,1);
54    for(int i=0;i<nb_mot;i++)
55    {
56        desc->SetElementName(i,0,MotorName(i));
57    }
58
59    output=new cvmatrix(self,desc,floatType);
60
61    self->AddDataToLog(output);
62}
63
64X4X8Multiplex_impl::~X4X8Multiplex_impl(void)
65{
66}
67
68void X4X8Multiplex_impl::UseDefaultPlot(void)
69{
70    Tab *plot_tab=new Tab(self->GetTabWidget(),"Values");
71    plots[0]=new DataPlot1D(plot_tab->NewRow(),"front left",0,1);
72    plots[1]=new DataPlot1D(plot_tab->LastRowLastCol(),"front right",0,1);
73    plots[2]=new DataPlot1D(plot_tab->NewRow(),"rear left",0,1);
74    plots[3]=new DataPlot1D(plot_tab->LastRowLastCol(),"rear right",0,1);
75
76    for(int i=0;i<4;i++) plots[i]->AddCurve(output->Element(i));
77
78    if(nb_mot==8)
79    {
80        for(int i=0;i<4;i++) plots[i]->AddCurve(output->Element(i+4),DataPlot::Blue);
81    }
82}
83
84void X4X8Multiplex_impl::UpdateFrom(const io_data *data)
85{
86    float u_roll,u_pitch,u_yaw,u_thrust;
87    float trim_roll,trim_pitch,trim_yaw;
88    float value[MAX_MOTORS];
89
90    cvmatrix* input=(cvmatrix*)data;
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    {
107        trim_yaw=-trim_yaw;
108        u_yaw=-u_yaw;
109    }
110
111if(nb_mot==2)
112{
113    //(top) front left
114    value[self->MultiplexValue(X4X8Multiplex::TopFrontLeft)]=Set(trim_pitch+trim_roll+trim_yaw,
115                 u_thrust+u_pitch+u_roll+u_yaw);
116
117    //(top) front right
118    value[self->MultiplexValue(X4X8Multiplex::TopFrontRight)]=Set(trim_pitch-trim_roll-trim_yaw,
119                 u_thrust+u_pitch-u_roll-u_yaw);
120}
121    if(nb_mot==4 || nb_mot==8)
122    {
123        //(top) front left
124        value[self->MultiplexValue(X4X8Multiplex::TopFrontLeft)]=Set(trim_pitch+trim_roll+trim_yaw,
125                     u_thrust+u_pitch+u_roll+u_yaw);
126
127        //(top) front right
128        value[self->MultiplexValue(X4X8Multiplex::TopFrontRight)]=Set(trim_pitch-trim_roll-trim_yaw,
129                     u_thrust+u_pitch-u_roll-u_yaw);
130
131        //(top) rear left
132        value[self->MultiplexValue(X4X8Multiplex::TopRearLeft)]=Set(-trim_pitch+trim_roll-trim_yaw,
133                     u_thrust-u_pitch+u_roll-u_yaw);
134
135        //(top) rear right
136        value[self->MultiplexValue(X4X8Multiplex::TopRearRight)]=Set(-trim_pitch-trim_roll+trim_yaw,
137                     u_thrust-u_pitch-u_roll+u_yaw);
138    }
139
140    if(nb_mot==8)
141    {
142        //bottom front left
143        value[self->MultiplexValue(X4X8Multiplex::BottomFrontLeft)]=Set(trim_pitch+trim_roll-trim_yaw,
144                     u_thrust+u_pitch+u_roll-u_yaw);
145
146        //bottom front right
147        value[self->MultiplexValue(X4X8Multiplex::BottomFrontRight)]=Set(trim_pitch-trim_roll+trim_yaw,
148                     u_thrust+u_pitch-u_roll+u_yaw);
149
150        //bottom rear left
151        value[self->MultiplexValue(X4X8Multiplex::BottomRearLeft)]=Set(-trim_pitch+trim_roll+trim_yaw,
152                     u_thrust-u_pitch+u_roll+u_yaw);
153
154        //bottom rear right
155        value[self->MultiplexValue(X4X8Multiplex::BottomRearRight)]=Set(-trim_pitch-trim_roll-trim_yaw,
156                     u_thrust-u_pitch-u_roll-u_yaw);
157
158    }
159
160    //on prend une fois pour toute le mutex et on fait des accès directs
161    output->GetMutex();
162    for(int i=0;i<nb_mot;i++) 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{
172    float value=trim;
173
174    if(u>0)
175    {
176        value+=sqrtf(u);
177    }
178
179    return value;
180}
181
182string X4X8Multiplex_impl::MotorName(int index)
183{
184    switch(nb_mot)
185    {
186        case 4:
187        {
188            switch(index)
189            {
190                case 0:
191                    return "front left";
192                case 1:
193                    return "front rigth";
194                case 2:
195                    return "rear left";
196                case 3:
197                    return "rear rigth";
198                default:
199                    return "unammed motor";
200            }
201        }
202        case 8:
203        {
204            switch(index)
205            {
206                case 0:
207                    return "top front left";
208                case 1:
209                    return "top front rigth";
210                case 2:
211                    return "top rear left";
212                case 3:
213                    return "top rear rigth";
214                case 4:
215                    return "bottom front left";
216                case 5:
217                    return "bottom front rigth";
218                case 6:
219                    return "bottom rear left";
220                case 7:
221                    return "bottom rear rigth";
222                default:
223                    return "unammed motor";
224            }
225        }
226        default:
227        {
228            return "unammed motor";
229        }
230    }
231}
Note: See TracBrowser for help on using the repository browser.