source: flair-src/trunk/lib/FlairFilter/src/PlaneMultiplex.cpp@ 461

Last change on this file since 461 was 461, checked in by Sanahuja Guillaume, 11 months ago

add plane files

File size: 4.5 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: 2022/01/05
6// filename: PlaneMultiplex.cpp
7//
8// author: Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: Class defining plane multiplexing
14//
15//
16/*********************************************************************/
17
18#include "PlaneMultiplex.h"
19#include <Matrix.h>
20#include <Tab.h>
21#include <GridLayout.h>
22#include <GroupBox.h>
23#include <ComboBox.h>
24#include <DoubleSpinBox.h>
25#include <DataPlot1D.h>
26#include <math.h>
27
28
29using std::string;
30using namespace flair::core;
31using namespace flair::gui;
32using namespace flair::filter;
33
34namespace flair {
35namespace filter {
36class PlaneMultiplexBldc : public IODevice {
37
38public:
39 PlaneMultiplexBldc(PlaneMultiplex *parent, string name):IODevice(parent,name) {
40 output = new Matrix(this, 1,1, floatType);
41 }
42 ~PlaneMultiplexBldc(){};
43
44
45private:
46 Matrix *output;
47 void UpdateFrom(const core::io_data *data) {
48 const Matrix* input = dynamic_cast<const Matrix*>(data);
49
50 if (!input) {
51 Warn("casting %s to Matrix failed\n",data->ObjectName().c_str());
52 return;
53 }
54 output->SetValue(0, 0, input->Value(0,0));
55
56 output->SetDataTime(data->DataTime());
57
58 ProcessUpdate(output);
59 }
60
61};
62
63class PlaneMultiplexServos : public IODevice {
64
65public:
66
67
68 PlaneMultiplexServos(PlaneMultiplex *parent, string name):IODevice(parent,name) {
69 output = new Matrix(this, 2,1, floatType);
70 }
71 ~PlaneMultiplexServos(){};
72
73
74private:
75 Matrix *output;
76 void UpdateFrom(const core::io_data *data) {
77 const Matrix* input = dynamic_cast<const Matrix*>(data);
78
79 if (!input) {
80 Warn("casting %s to Matrix failed\n",data->ObjectName().c_str());
81 return;
82 }
83 input->GetMutex();
84 output->GetMutex();
85 for (int i = 0; i < 2; i++) output->SetValueNoMutex(i, 0, input->ValueNoMutex(1+i,0));
86 output->ReleaseMutex();
87 input->ReleaseMutex();
88
89 output->SetDataTime(data->DataTime());
90
91 ProcessUpdate(output);
92 }
93
94
95};
96
97} // end namespace filter
98} // end namespace flair
99
100PlaneMultiplex::PlaneMultiplex(string name) :UavMultiplex(name){
101
102 bldc=new PlaneMultiplexBldc(this,"bldc_multiplex");
103 servos=new PlaneMultiplexServos(this,"servos_multiplex");
104
105 MatrixDescriptor *desc = new MatrixDescriptor(6, 1);
106 for (int i = 0; i < 3; i++) {
107 desc->SetElementName(i, 0, ActuatorName(i));
108 }
109
110 output = new Matrix(this, desc, floatType);
111 delete desc;
112 AddDataToLog(output);
113}
114
115PlaneMultiplex::~PlaneMultiplex(void) {}
116
117IODevice* PlaneMultiplex::GetBldcMultiplex(void) const {
118 return bldc;
119}
120
121IODevice* PlaneMultiplex::GetServosMultiplex(void) const {
122 return servos;
123}
124
125
126void PlaneMultiplex::UseDefaultPlot(void) {
127 Tab *plot_tab = new Tab(GetTabWidget(), "Values");
128 plots[0] = new DataPlot1D(plot_tab->NewRow(), ActuatorName(0), 0, 1);
129 plots[1] = new DataPlot1D(plot_tab->LastRowLastCol(), ActuatorName(1), 0, 1);
130 plots[2] = new DataPlot1D(plot_tab->NewRow(), ActuatorName(2), 0, 1);
131
132 for (int i = 0; i < 3; i++) {
133 plots[i]->AddCurve(output->Element(i));
134 }
135}
136
137void PlaneMultiplex::UpdateFrom(const io_data *data) {
138 float u_roll, u_pitch, u_yaw, u_thrust;
139 float trim_roll, trim_pitch, trim_yaw;
140 float value[3];
141
142 const Matrix* input = dynamic_cast<const Matrix*>(data);
143
144 if (!input) {
145 Warn("casting %s to Matrix failed\n",data->ObjectName().c_str());
146 return;
147 }
148
149 // on prend une fois pour toute le mutex et on fait des accès directs
150 input->GetMutex();
151
152 u_roll = input->ValueNoMutex(0, 0);
153 u_pitch = input->ValueNoMutex(1, 0);
154 u_yaw = input->ValueNoMutex(2, 0);
155 u_thrust = input->ValueNoMutex(3, 0);
156 trim_roll = input->ValueNoMutex(4, 0);
157 trim_pitch = input->ValueNoMutex(5, 0);
158 trim_yaw = input->ValueNoMutex(6, 0);
159
160 input->ReleaseMutex();
161
162 //font motor
163 value[0]=0;
164
165 //aileron right
166 value[1]=0;
167
168 //aileron left
169 value[2]=0;
170
171 // on prend une fois pour toute le mutex et on fait des accès directs
172 output->GetMutex();
173 for (int i = 0; i < 3; i++)
174 output->SetValueNoMutex(i, 0, value[i]);
175 output->ReleaseMutex();
176
177 output->SetDataTime(data->DataTime());
178
179 ProcessUpdate(output);
180}
181
182string PlaneMultiplex::ActuatorName(int index) {
183
184 switch (index) {
185 case 0:
186 return "front motor";
187 case 1:
188 return "right aileron";
189 case 2:
190 return "left aileron";
191 default:
192 return "unammed actuator";
193 }
194}
195
196uint8_t PlaneMultiplex::MotorsCount(void) const { return 3; }
Note: See TracBrowser for help on using the repository browser.