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

Last change on this file was 421, checked in by Sanahuja Guillaume, 8 months ago

add a permanent trim, usefull when uav is not well balanced (ie intel aero)

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