[10] | 1 | // %flair:license{
|
---|
[15] | 2 | // This file is part of the Flair framework distributed under the
|
---|
| 3 | // CECILL-C License, Version 1.0.
|
---|
[10] | 4 | // %flair:license}
|
---|
[7] | 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"
|
---|
[214] | 20 | #include <Matrix.h>
|
---|
[7] | 21 | #include <Tab.h>
|
---|
| 22 | #include <GridLayout.h>
|
---|
| 23 | #include <GroupBox.h>
|
---|
| 24 | #include <ComboBox.h>
|
---|
| 25 | #include <DataPlot1D.h>
|
---|
[421] | 26 | #include <DoubleSpinBox.h>
|
---|
[7] | 27 | #include <math.h>
|
---|
| 28 |
|
---|
| 29 | #define MAX_MOTORS 8
|
---|
| 30 |
|
---|
| 31 | using std::string;
|
---|
| 32 | using namespace flair::core;
|
---|
| 33 | using namespace flair::gui;
|
---|
| 34 | using namespace flair::filter;
|
---|
| 35 |
|
---|
[15] | 36 | X4X8Multiplex_impl::X4X8Multiplex_impl(flair::filter::X4X8Multiplex *self,
|
---|
| 37 | int nb_mot) {
|
---|
| 38 | this->nb_mot = nb_mot;
|
---|
| 39 | this->self = self;
|
---|
[7] | 40 |
|
---|
[15] | 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 | }
|
---|
[106] | 50 | pas->AddItem("counter clockwise");
|
---|
| 51 | pas->AddItem("clockwise");
|
---|
[421] | 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);
|
---|
[7] | 55 |
|
---|
[318] | 56 | MatrixDescriptor *desc = new MatrixDescriptor(nb_mot, 1);
|
---|
[15] | 57 | for (int i = 0; i < nb_mot; i++) {
|
---|
| 58 | desc->SetElementName(i, 0, MotorName(i));
|
---|
| 59 | }
|
---|
[7] | 60 |
|
---|
[214] | 61 | output = new Matrix(self, desc, floatType);
|
---|
[148] | 62 | delete desc;
|
---|
[15] | 63 | self->AddDataToLog(output);
|
---|
[7] | 64 | }
|
---|
| 65 |
|
---|
[15] | 66 | X4X8Multiplex_impl::~X4X8Multiplex_impl(void) {}
|
---|
[7] | 67 |
|
---|
[15] | 68 | void 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);
|
---|
[7] | 74 |
|
---|
[15] | 75 | for (int i = 0; i < 4; i++)
|
---|
| 76 | plots[i]->AddCurve(output->Element(i));
|
---|
[7] | 77 |
|
---|
[15] | 78 | if (nb_mot == 8) {
|
---|
| 79 | for (int i = 0; i < 4; i++)
|
---|
| 80 | plots[i]->AddCurve(output->Element(i + 4), DataPlot::Blue);
|
---|
| 81 | }
|
---|
[7] | 82 | }
|
---|
| 83 |
|
---|
[15] | 84 | void 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];
|
---|
[7] | 88 |
|
---|
[214] | 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 | }
|
---|
[7] | 95 |
|
---|
[15] | 96 | // on prend une fois pour toute le mutex et on fait des accès directs
|
---|
| 97 | input->GetMutex();
|
---|
[7] | 98 |
|
---|
[15] | 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);
|
---|
[421] | 103 | trim_roll = input->ValueNoMutex(4, 0)+permanent_trim_roll->Value();
|
---|
| 104 | trim_pitch = input->ValueNoMutex(5, 0)+permanent_trim_pitch->Value();
|
---|
[15] | 105 | trim_yaw = input->ValueNoMutex(6, 0);
|
---|
[7] | 106 |
|
---|
[15] | 107 | input->ReleaseMutex();
|
---|
[7] | 108 |
|
---|
[15] | 109 | if (pas->CurrentIndex() == 1) {
|
---|
| 110 | trim_yaw = -trim_yaw;
|
---|
| 111 | u_yaw = -u_yaw;
|
---|
| 112 | }
|
---|
[7] | 113 |
|
---|
[15] | 114 | if (nb_mot == 2) {
|
---|
[7] | 115 | //(top) front left
|
---|
[15] | 116 | value[self->MultiplexValue(X4X8Multiplex::TopFrontLeft)] = Set(
|
---|
| 117 | trim_pitch + trim_roll + trim_yaw, u_thrust + u_pitch + u_roll + u_yaw);
|
---|
[7] | 118 |
|
---|
| 119 | //(top) front right
|
---|
[15] | 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);
|
---|
[7] | 127 |
|
---|
[15] | 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);
|
---|
[7] | 131 |
|
---|
[15] | 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);
|
---|
[7] | 136 |
|
---|
[15] | 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 | }
|
---|
[7] | 142 |
|
---|
[15] | 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);
|
---|
[7] | 147 |
|
---|
[15] | 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);
|
---|
[7] | 151 |
|
---|
[15] | 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);
|
---|
[7] | 156 |
|
---|
[15] | 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);
|
---|
[7] | 172 | }
|
---|
| 173 |
|
---|
[15] | 174 | float X4X8Multiplex_impl::Set(float trim, float u) {
|
---|
| 175 | float value = trim;
|
---|
[7] | 176 |
|
---|
[15] | 177 | if (u > 0) {
|
---|
| 178 | value += sqrtf(u);
|
---|
| 179 | }
|
---|
[7] | 180 |
|
---|
[15] | 181 | return value;
|
---|
[7] | 182 | }
|
---|
| 183 |
|
---|
[15] | 184 | string 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";
|
---|
[7] | 198 | }
|
---|
[15] | 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 | }
|
---|
[7] | 224 | }
|
---|