Ignore:
Timestamp:
04/08/16 15:40:57 (7 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairFilter/src/X4X8Multiplex_impl.cpp

    r10 r15  
    3333using namespace flair::filter;
    3434
    35 X4X8Multiplex_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:");
     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("normal");
     50  pas->AddItem("inverted");
     51
     52  cvmatrix_descriptor *desc = new cvmatrix_descriptor(nb_mot, 1);
     53  for (int i = 0; i < nb_mot; i++) {
     54    desc->SetElementName(i, 0, MotorName(i));
     55  }
     56
     57  output = new cvmatrix(self, desc, floatType);
     58
     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  cvmatrix *input = (cvmatrix *)data;
     86
     87  // on prend une fois pour toute le mutex et on fait des accès directs
     88  input->GetMutex();
     89
     90  u_roll = input->ValueNoMutex(0, 0);
     91  u_pitch = input->ValueNoMutex(1, 0);
     92  u_yaw = input->ValueNoMutex(2, 0);
     93  u_thrust = input->ValueNoMutex(3, 0);
     94  trim_roll = input->ValueNoMutex(4, 0);
     95  trim_pitch = input->ValueNoMutex(5, 0);
     96  trim_yaw = input->ValueNoMutex(6, 0);
     97
     98  input->ReleaseMutex();
     99
     100  if (pas->CurrentIndex() == 1) {
     101    trim_yaw = -trim_yaw;
     102    u_yaw = -u_yaw;
     103  }
     104
     105  if (nb_mot == 2) {
     106    //(top) front left
     107    value[self->MultiplexValue(X4X8Multiplex::TopFrontLeft)] = Set(
     108        trim_pitch + trim_roll + trim_yaw, u_thrust + u_pitch + u_roll + u_yaw);
     109
     110    //(top) front right
     111    value[self->MultiplexValue(X4X8Multiplex::TopFrontRight)] = Set(
     112        trim_pitch - trim_roll - trim_yaw, u_thrust + u_pitch - u_roll - u_yaw);
     113  }
     114  if (nb_mot == 4 || nb_mot == 8) {
     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    //(top) rear left
     124    value[self->MultiplexValue(X4X8Multiplex::TopRearLeft)] =
     125        Set(-trim_pitch + trim_roll - trim_yaw,
     126            u_thrust - u_pitch + u_roll - u_yaw);
     127
     128    //(top) rear right
     129    value[self->MultiplexValue(X4X8Multiplex::TopRearRight)] =
     130        Set(-trim_pitch - trim_roll + trim_yaw,
     131            u_thrust - u_pitch - u_roll + u_yaw);
     132  }
     133
     134  if (nb_mot == 8) {
     135    // bottom front left
     136    value[self->MultiplexValue(X4X8Multiplex::BottomFrontLeft)] = Set(
     137        trim_pitch + trim_roll - trim_yaw, u_thrust + u_pitch + u_roll - u_yaw);
     138
     139    // bottom front right
     140    value[self->MultiplexValue(X4X8Multiplex::BottomFrontRight)] = Set(
     141        trim_pitch - trim_roll + trim_yaw, u_thrust + u_pitch - u_roll + u_yaw);
     142
     143    // bottom rear left
     144    value[self->MultiplexValue(X4X8Multiplex::BottomRearLeft)] =
     145        Set(-trim_pitch + trim_roll + trim_yaw,
     146            u_thrust - u_pitch + u_roll + u_yaw);
     147
     148    // bottom rear right
     149    value[self->MultiplexValue(X4X8Multiplex::BottomRearRight)] =
     150        Set(-trim_pitch - trim_roll - trim_yaw,
     151            u_thrust - u_pitch - u_roll - u_yaw);
     152  }
     153
     154  // on prend une fois pour toute le mutex et on fait des accès directs
     155  output->GetMutex();
     156  for (int i = 0; i < nb_mot; i++)
     157    output->SetValueNoMutex(i, 0, value[i]);
     158  output->ReleaseMutex();
     159
     160  output->SetDataTime(data->DataTime());
     161
     162  self->ProcessUpdate(output);
     163}
     164
     165float X4X8Multiplex_impl::Set(float trim, float u) {
     166  float value = trim;
     167
     168  if (u > 0) {
     169    value += sqrtf(u);
     170  }
     171
     172  return value;
     173}
     174
     175string X4X8Multiplex_impl::MotorName(int index) {
     176  switch (nb_mot) {
     177  case 4: {
     178    switch (index) {
     179    case 0:
     180      return "front left";
     181    case 1:
     182      return "front rigth";
     183    case 2:
     184      return "rear left";
     185    case 3:
     186      return "rear rigth";
     187    default:
     188      return "unammed motor";
    44189    }
    45     else
    46     {
    47         GroupBox *groupbox=new GroupBox(self->GetLayout()->NewRow(),"x8 multiplex");
    48         pas=new ComboBox(groupbox->NewRow(),"top front left blade pitch:");
     190  }
     191  case 8: {
     192    switch (index) {
     193    case 0:
     194      return "top front left";
     195    case 1:
     196      return "top front rigth";
     197    case 2:
     198      return "top rear left";
     199    case 3:
     200      return "top rear rigth";
     201    case 4:
     202      return "bottom front left";
     203    case 5:
     204      return "bottom front rigth";
     205    case 6:
     206      return "bottom rear left";
     207    case 7:
     208      return "bottom rear rigth";
     209    default:
     210      return "unammed motor";
    49211    }
    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 
    64 X4X8Multiplex_impl::~X4X8Multiplex_impl(void)
    65 {
    66 }
    67 
    68 void 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 
    84 void 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 
    111 if(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 
    170 float 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 
    182 string 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 }
     212  }
     213  default: { return "unammed motor"; }
     214  }
     215}
Note: See TracChangeset for help on using the changeset viewer.