Ignore:
Timestamp:
Apr 8, 2016, 3:40:57 PM (5 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairSensorActuator/src/BlCtrlV2_x4_speed.cpp

    r3 r15  
    3838using namespace flair::gui;
    3939
    40 namespace flair
    41 {
    42 namespace actuator
    43 {
    44 BlCtrlV2_x4_speed::BlCtrlV2_x4_speed(FrameworkManager* parent,string name,I2cPort* i2cport,uint8_t base_address,uint8_t priority) : Thread(parent,name,priority),IODevice(parent,name)
    45 {
    46     this->i2cport=i2cport;
    47         slave_address=base_address;
    48         tested_motor=-1;
    49         enabled=false;
    50         int_av_g=0;
    51         int_av_d=0;
    52         int_ar_g=0;
    53         int_ar_d=0;
    54 
    55         //flight time
    56         FILE *file;
    57     file=fopen("/etc/flight_time","r");
    58     if (file == NULL)
    59     {
    60         Printf("fichier d'info de vol vide\n");
    61         time_sec=0;
     40namespace flair {
     41namespace actuator {
     42BlCtrlV2_x4_speed::BlCtrlV2_x4_speed(FrameworkManager *parent, string name,
     43                                     I2cPort *i2cport, uint8_t base_address,
     44                                     uint8_t priority)
     45    : Thread(parent, name, priority), IODevice(parent, name) {
     46  this->i2cport = i2cport;
     47  slave_address = base_address;
     48  tested_motor = -1;
     49  enabled = false;
     50  int_av_g = 0;
     51  int_av_d = 0;
     52  int_ar_g = 0;
     53  int_ar_d = 0;
     54
     55  // flight time
     56  FILE *file;
     57  file = fopen("/etc/flight_time", "r");
     58  if (file == NULL) {
     59    Printf("fichier d'info de vol vide\n");
     60    time_sec = 0;
     61  } else {
     62    char ligne[32];
     63    fgets(ligne, 32, file);
     64    time_sec = atoi(ligne);
     65    Printf("temps de vol total: %is = %imin = %ih\n", time_sec, time_sec / 60,
     66           time_sec / 3600);
     67    fclose(file);
     68  }
     69
     70  // station sol
     71  main_tab = new Tab(parent->GetTabWidget(), name);
     72  tab = new TabWidget(main_tab->NewRow(), name);
     73  Tab *sensor_tab = new Tab(tab, "Reglages");
     74  reglages_groupbox = new GroupBox(sensor_tab->NewRow(), name);
     75  poles = new SpinBox(reglages_groupbox->NewRow(), "nb poles", 0, 255, 1);
     76  kp = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "kp", 0., 255,
     77                         0.001, 4);
     78  ki = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "ki", 0., 255,
     79                         0.001, 4);
     80  min = new SpinBox(reglages_groupbox->NewRow(), "min pwm", 0., 2048, 1);
     81  max =
     82      new SpinBox(reglages_groupbox->LastRowLastCol(), "max pwm", 0., 2048, 1);
     83  test = new SpinBox(reglages_groupbox->LastRowLastCol(), "test value", 0.,
     84                     2048, 1);
     85  start_value = new SpinBox(reglages_groupbox->NewRow(), "valeur demarrage", 0,
     86                            10000, 10);
     87  trim = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "pas decollage",
     88                           0, 1000, .1);
     89
     90  av_g = new ComboBox(reglages_groupbox->NewRow(), "avant gauche");
     91  av_g->AddItem("1");
     92  av_g->AddItem("2");
     93  av_g->AddItem("3");
     94  av_g->AddItem("4");
     95  button_avg = new PushButton(reglages_groupbox->LastRowLastCol(), "test avg");
     96
     97  av_d = new ComboBox(reglages_groupbox->LastRowLastCol(), "avant droite:");
     98  av_d->AddItem("1");
     99  av_d->AddItem("2");
     100  av_d->AddItem("3");
     101  av_d->AddItem("4");
     102  button_avd = new PushButton(reglages_groupbox->LastRowLastCol(), "test avd");
     103
     104  ar_g = new ComboBox(reglages_groupbox->NewRow(), "arriere gauche:");
     105  ar_g->AddItem("1");
     106  ar_g->AddItem("2");
     107  ar_g->AddItem("3");
     108  ar_g->AddItem("4");
     109  button_arg = new PushButton(reglages_groupbox->LastRowLastCol(), "test arg");
     110
     111  ar_d = new ComboBox(reglages_groupbox->LastRowLastCol(), "arriere droite:");
     112  ar_d->AddItem("1");
     113  ar_d->AddItem("2");
     114  ar_d->AddItem("3");
     115  ar_d->AddItem("4");
     116  button_ard = new PushButton(reglages_groupbox->LastRowLastCol(), "test ard");
     117
     118  pas = new ComboBox(reglages_groupbox->NewRow(), "pas helice avant gauche:");
     119  pas->AddItem("normal");
     120  pas->AddItem("inverse");
     121
     122  input = new cvmatrix((IODevice *)this, 8, 1, floatType);
     123
     124  cvmatrix_descriptor *desc = new cvmatrix_descriptor(4, 2);
     125  desc->SetElementName(0, 0, "avant gauche");
     126  desc->SetElementName(1, 0, "arriere droite");
     127  desc->SetElementName(2, 0, "avant droite");
     128  desc->SetElementName(3, 0, "arriere gauche");
     129
     130  desc->SetElementName(0, 1, "cons avant gauche");
     131  desc->SetElementName(1, 1, "cons arriere droite");
     132  desc->SetElementName(2, 1, "cons avant droite");
     133  desc->SetElementName(3, 1, "cons arriere gauche");
     134  output = new cvmatrix((IODevice *)this, desc, floatType);
     135
     136  /*
     137
     138  //le 3ieme lu est la tension batteire
     139          if(i2c_mutex!=NULL) i2c_mutex->GetMutex();
     140      uint16_t pwm_moteur;
     141      pwm_moteur=0;
     142      ssize_t read;
     143      uint8_t rx[8];
     144      SetSlave(slave_address);
     145
     146      for(int j=0;j<10;j++)
     147      {
     148
     149
     150          WriteValue(pwm_moteur);
     151
     152
     153          read = rt_dev_read(i2c_fd, rx, sizeof(rx));
     154
     155          if(read<0)
     156          {
     157              rt_printf("BlCtrlV2_x4_speed::BlCtrlV2_x4_speed: %s, erreur
     158  rt_dev_read (%s)\n",IODevice::ObjectName().c_str(),strerror(-read));
     159          }
     160          else if (read != sizeof(rx))
     161          {
     162              rt_printf("BlCtrlV2_x4_speed::BlCtrlV2_x4_speed: %s, erreur
     163  rt_dev_read %i/2\n",IODevice::ObjectName().c_str(),read);
     164
     165          }
     166          for(int i=0;i<sizeof(rx);i++) printf("%i ",rx[i]);
     167
     168          printf("\n");
     169
     170      }
     171
     172      if(i2c_mutex!=NULL) i2c_mutex->ReleaseMutex();*/
     173}
     174
     175BlCtrlV2_x4_speed::~BlCtrlV2_x4_speed(void) {
     176  SafeStop();
     177  Join();
     178  delete main_tab;
     179}
     180
     181void BlCtrlV2_x4_speed::UseDefaultPlot(void) {
     182  Tab *plot_tab = new Tab(tab, "Mesures");
     183  DataPlot1D *av_g_plot = new DataPlot1D(plot_tab->NewRow(), "avg", 0, 10000);
     184  av_g_plot->AddCurve(output->Element(0, 0));
     185  av_g_plot->AddCurve(output->Element(0, 1), DataPlot::Blue);
     186  DataPlot1D *av_d_plot =
     187      new DataPlot1D(plot_tab->LastRowLastCol(), "avd", 0, 10000);
     188  av_d_plot->AddCurve(output->Element(2, 0));
     189  av_d_plot->AddCurve(output->Element(2, 1), DataPlot::Blue);
     190  DataPlot1D *ar_g_plot = new DataPlot1D(plot_tab->NewRow(), "arg", 0, 10000);
     191  ar_g_plot->AddCurve(output->Element(3, 0));
     192  ar_g_plot->AddCurve(output->Element(3, 1), DataPlot::Blue);
     193  DataPlot1D *ar_d_plot =
     194      new DataPlot1D(plot_tab->LastRowLastCol(), "ard", 0, 10000);
     195  ar_d_plot->AddCurve(output->Element(1, 0));
     196  ar_d_plot->AddCurve(output->Element(1, 1), DataPlot::Blue);
     197}
     198
     199float BlCtrlV2_x4_speed::TrimValue(void) { return (float)trim->Value(); }
     200
     201int BlCtrlV2_x4_speed::StartValue(void) { return start_value->Value(); }
     202
     203void BlCtrlV2_x4_speed::Run(void) {
     204  WarnUponSwitches(true);
     205
     206  SetPeriodUS(TAU_US);
     207
     208  while (!ToBeStopped()) {
     209    WaitPeriod();
     210
     211    Update();
     212  }
     213
     214  WarnUponSwitches(false);
     215}
     216
     217void BlCtrlV2_x4_speed::Update(void) {
     218  float u_roll, u_pitch, u_yaw, u_gaz;
     219  float trim_roll, trim_pitch, trim_yaw;
     220  float pwm[4];
     221  uint16_t pwm_moteur[4];
     222
     223  // on prend une fois pour toute le mutex et on fait des accès directs
     224  input->GetMutex();
     225
     226  u_roll = input->ValueNoMutex(0, 0);
     227  u_pitch = input->ValueNoMutex(1, 0);
     228  u_yaw = input->ValueNoMutex(2, 0);
     229  u_gaz =
     230      input->ValueNoMutex(3, 0) +
     231      input->ValueNoMutex(7, 0) * input->ValueNoMutex(7, 0); // ugaz+trim*trim
     232  trim_roll = input->ValueNoMutex(4, 0);
     233  trim_pitch = input->ValueNoMutex(5, 0);
     234  trim_yaw = input->ValueNoMutex(6, 0);
     235
     236  input->ReleaseMutex();
     237
     238  if (pas->CurrentIndex() == 1) {
     239    trim_yaw = -trim_yaw;
     240    u_yaw = -u_yaw;
     241  }
     242
     243  // rt_printf("%f %f %f %f\n",u_roll,u_pitch,u_yaw,u_gaz);
     244  // if(u_gaz!=0) rt_printf("gaz: %f\n",u_gaz);
     245
     246  // avant gauche
     247  if (u_gaz + u_pitch + u_roll + u_yaw > 0) {
     248    pwm[0] = trim_pitch + trim_roll + trim_yaw +
     249             sqrtf(u_gaz + u_pitch + u_roll + u_yaw);
     250  } else {
     251    pwm[0] = trim_pitch + trim_roll + trim_yaw;
     252  }
     253
     254  // arriere gauche
     255  if (u_gaz - u_pitch + u_roll - u_yaw > 0) {
     256    pwm[3] = -trim_pitch + trim_roll - trim_yaw +
     257             sqrtf(u_gaz - u_pitch + u_roll - u_yaw);
     258  } else {
     259    pwm[3] = -trim_pitch + trim_roll - trim_yaw;
     260  }
     261
     262  // arriere droit
     263  if (u_gaz - u_pitch - u_roll + u_yaw > 0) {
     264    pwm[1] = -trim_pitch - trim_roll + trim_yaw +
     265             sqrtf(u_gaz - u_pitch - u_roll + u_yaw);
     266  } else {
     267    pwm[1] = -trim_pitch - trim_roll + trim_yaw;
     268  }
     269
     270  // avant droit
     271  if (u_gaz + u_pitch - u_roll - u_yaw > 0) {
     272    pwm[2] = trim_pitch - trim_roll - trim_yaw +
     273             sqrtf(u_gaz + u_pitch - u_roll - u_yaw);
     274  } else {
     275    pwm[2] = trim_pitch - trim_roll - trim_yaw;
     276  }
     277
     278  int_av_g += ki->Value() * (pwm[0] - speed_av_g);
     279  pwm[0] = kp->Value() * (pwm[0] - speed_av_g) + int_av_g;
     280
     281  int_ar_g += ki->Value() * (pwm[3] - speed_ar_g);
     282  pwm[3] = kp->Value() * (pwm[3] - speed_ar_g) + int_ar_g;
     283
     284  int_ar_d += ki->Value() * (pwm[1] - speed_ar_d);
     285  pwm[1] = kp->Value() * (pwm[1] - speed_ar_d) + int_ar_d;
     286
     287  int_av_d += ki->Value() * (pwm[2] - speed_av_d);
     288  pwm[2] = kp->Value() * (pwm[2] - speed_av_d) + int_av_d;
     289
     290  // rt_printf("%f\n",pwm[0]);
     291  for (int i = 0; i < 4; i++)
     292    pwm_moteur[i] = SatPWM(pwm[i], min->Value(), max->Value());
     293
     294  if (button_avg->Clicked() == true) {
     295    tested_motor = 0;
     296    StartTest();
     297  }
     298  if (button_avd->Clicked() == true) {
     299    tested_motor = 2;
     300    StartTest();
     301  }
     302  if (button_arg->Clicked() == true) {
     303    tested_motor = 3;
     304    StartTest();
     305  }
     306  if (button_ard->Clicked() == true) {
     307    tested_motor = 1;
     308    StartTest();
     309  }
     310
     311  if (tested_motor != -1) {
     312    for (int i = 0; i < 4; i++) {
     313      pwm_moteur[i] = 0;
    62314    }
    63     else
    64     {
    65         char ligne[32];
    66         fgets(ligne, 32, file);
    67         time_sec=atoi(ligne);
    68         Printf("temps de vol total: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
    69         fclose(file);
     315    pwm_moteur[tested_motor] = (uint16_t)test->Value();
     316
     317    if (GetTime() > (start_time + 2 * 1000000000))
     318      StopTest();
     319  }
     320
     321  i2cport->GetMutex();
     322
     323  if (enabled == true) {
     324    i2cport->SetSlave(slave_address + av_g->CurrentIndex());
     325    WriteValue(pwm_moteur[0]);
     326
     327    i2cport->SetSlave(slave_address + av_d->CurrentIndex());
     328    WriteValue(pwm_moteur[2]);
     329
     330    i2cport->SetSlave(slave_address + ar_g->CurrentIndex());
     331    WriteValue(pwm_moteur[3]);
     332
     333    i2cport->SetSlave(slave_address + ar_d->CurrentIndex());
     334    WriteValue(pwm_moteur[1]);
     335
     336  } else {
     337    for (int i = 0; i < 4; i++) {
     338      i2cport->SetSlave(slave_address + i);
     339      WriteValue(0);
    70340    }
    71 
    72     //station sol
    73     main_tab=new Tab(parent->GetTabWidget(),name);
    74     tab=new TabWidget(main_tab->NewRow(),name);
    75         Tab *sensor_tab=new Tab(tab,"Reglages");
    76         reglages_groupbox=new GroupBox(sensor_tab->NewRow(),name);
    77             poles=new SpinBox(reglages_groupbox->NewRow(),"nb poles",0,255,1);
    78             kp=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"kp",0.,255,0.001,4);
    79             ki=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"ki",0.,255,0.001,4);
    80             min=new SpinBox(reglages_groupbox->NewRow(),"min pwm",0.,2048,1);
    81             max=new SpinBox(reglages_groupbox->LastRowLastCol(),"max pwm",0.,2048,1);
    82             test=new SpinBox(reglages_groupbox->LastRowLastCol(),"test value",0.,2048,1);
    83             start_value=new SpinBox(reglages_groupbox->NewRow(),"valeur demarrage",0,10000,10);
    84             trim=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"pas decollage",0,1000,.1);
    85 
    86             av_g=new ComboBox(reglages_groupbox->NewRow(),"avant gauche");
    87             av_g->AddItem("1");
    88             av_g->AddItem("2");
    89             av_g->AddItem("3");
    90             av_g->AddItem("4");
    91             button_avg=new PushButton(reglages_groupbox->LastRowLastCol(),"test avg");
    92 
    93             av_d=new ComboBox(reglages_groupbox->LastRowLastCol(),"avant droite:");
    94             av_d->AddItem("1");
    95             av_d->AddItem("2");
    96             av_d->AddItem("3");
    97             av_d->AddItem("4");
    98             button_avd=new PushButton(reglages_groupbox->LastRowLastCol(),"test avd");
    99 
    100             ar_g=new ComboBox(reglages_groupbox->NewRow(),"arriere gauche:");
    101             ar_g->AddItem("1");
    102             ar_g->AddItem("2");
    103             ar_g->AddItem("3");
    104             ar_g->AddItem("4");
    105             button_arg=new PushButton(reglages_groupbox->LastRowLastCol(),"test arg");
    106 
    107             ar_d=new ComboBox(reglages_groupbox->LastRowLastCol(),"arriere droite:");
    108             ar_d->AddItem("1");
    109             ar_d->AddItem("2");
    110             ar_d->AddItem("3");
    111             ar_d->AddItem("4");
    112             button_ard=new PushButton(reglages_groupbox->LastRowLastCol(),"test ard");
    113 
    114             pas=new ComboBox(reglages_groupbox->NewRow(),"pas helice avant gauche:");
    115             pas->AddItem("normal");
    116             pas->AddItem("inverse");
    117 
    118     input=new cvmatrix((IODevice*)this,8,1,floatType);
    119 
    120     cvmatrix_descriptor* desc=new cvmatrix_descriptor(4,2);
    121     desc->SetElementName(0,0,"avant gauche");
    122         desc->SetElementName(1,0,"arriere droite");
    123         desc->SetElementName(2,0,"avant droite");
    124         desc->SetElementName(3,0,"arriere gauche");
    125 
    126         desc->SetElementName(0,1,"cons avant gauche");
    127         desc->SetElementName(1,1,"cons arriere droite");
    128         desc->SetElementName(2,1,"cons avant droite");
    129         desc->SetElementName(3,1,"cons arriere gauche");
    130     output=new cvmatrix((IODevice*)this,desc,floatType);
    131 
    132 
    133 
    134 /*
    135 
    136 //le 3ieme lu est la tension batteire
    137         if(i2c_mutex!=NULL) i2c_mutex->GetMutex();
    138     uint16_t pwm_moteur;
    139     pwm_moteur=0;
    140     ssize_t read;
    141     uint8_t rx[8];
    142     SetSlave(slave_address);
    143 
    144     for(int j=0;j<10;j++)
    145     {
    146 
    147 
    148         WriteValue(pwm_moteur);
    149 
    150 
    151         read = rt_dev_read(i2c_fd, rx, sizeof(rx));
    152 
    153         if(read<0)
    154         {
    155             rt_printf("BlCtrlV2_x4_speed::BlCtrlV2_x4_speed: %s, erreur rt_dev_read (%s)\n",IODevice::ObjectName().c_str(),strerror(-read));
    156         }
    157         else if (read != sizeof(rx))
    158         {
    159             rt_printf("BlCtrlV2_x4_speed::BlCtrlV2_x4_speed: %s, erreur rt_dev_read %i/2\n",IODevice::ObjectName().c_str(),read);
    160 
    161         }
    162         for(int i=0;i<sizeof(rx);i++) printf("%i ",rx[i]);
    163 
    164         printf("\n");
    165 
     341    int_av_g = 0;
     342    int_av_d = 0;
     343    int_ar_g = 0;
     344    int_ar_d = 0;
     345  }
     346
     347  i2cport->SetSlave(slave_address + av_g->CurrentIndex());
     348  speed_av_g = GetSpeed();
     349
     350  i2cport->SetSlave(slave_address + av_d->CurrentIndex());
     351  speed_av_d = GetSpeed();
     352
     353  i2cport->SetSlave(slave_address + ar_g->CurrentIndex());
     354  speed_ar_g = GetSpeed();
     355
     356  i2cport->SetSlave(slave_address + ar_d->CurrentIndex());
     357  speed_ar_d = GetSpeed();
     358
     359  i2cport->ReleaseMutex();
     360
     361  // on prend une fois pour toute le mutex et on fait des accès directs
     362  output->GetMutex();
     363  output->SetValueNoMutex(0, 0, speed_av_g);
     364  output->SetValueNoMutex(1, 0, speed_ar_d);
     365  output->SetValueNoMutex(2, 0, speed_av_d);
     366  output->SetValueNoMutex(3, 0, speed_ar_g);
     367  // rt_printf("%i %i %i
     368  // %i\n",pwm_moteur[0],pwm_moteur[1],pwm_moteur[2],pwm_moteur[3]);
     369  output->ReleaseMutex();
     370
     371  output->SetDataTime(GetTime());
     372  ProcessUpdate(output);
     373}
     374
     375void BlCtrlV2_x4_speed::StartTest(void) {
     376  start_time = GetTime();
     377  SetEnabled(true);
     378}
     379
     380void BlCtrlV2_x4_speed::StopTest(void) {
     381  SetEnabled(false);
     382  tested_motor = -1;
     383}
     384
     385uint16_t BlCtrlV2_x4_speed::SatPWM(float vel_cons, uint16_t min, uint16_t max) {
     386  uint16_t sat_value = (uint16_t)vel_cons;
     387
     388  if (vel_cons > ((float)sat_value + 0.5))
     389    sat_value++;
     390
     391  if (vel_cons < (float)min)
     392    sat_value = min;
     393  if (vel_cons > (float)max)
     394    sat_value = max;
     395
     396  return sat_value;
     397}
     398
     399void BlCtrlV2_x4_speed::LockUserInterface(void) {
     400  reglages_groupbox->setEnabled(false);
     401}
     402
     403void BlCtrlV2_x4_speed::UnlockUserInterface(void) {
     404  reglages_groupbox->setEnabled(true);
     405}
     406
     407void BlCtrlV2_x4_speed::SetEnabled(bool status) {
     408  enabled = status;
     409  if (enabled == true) {
     410    LockUserInterface();
     411
     412    flight_start_time = GetTime();
     413  } else {
     414    UnlockUserInterface();
     415
     416    Time now = GetTime();
     417    int t_sec;
     418    FILE *file;
     419    char ligne[32];
     420
     421    t_sec = (now - flight_start_time) / 1000000000;
     422    time_sec += t_sec;
     423
     424    Printf("temps de vol: %is = %imin\n", t_sec, t_sec / 60);
     425    Printf("temps de vol total: %is = %imin = %ih\n", time_sec, time_sec / 60,
     426           time_sec / 3600);
     427
     428    file = fopen("/etc/flight_time", "w");
     429    if (file == NULL) {
     430      Thread::Err("Erreur a l'ouverture du fichier d'info vol\n");
     431    } else {
     432      sprintf(ligne, "%i", time_sec);
     433      fputs(ligne, file);
     434      fclose(file);
    166435    }
    167 
    168     if(i2c_mutex!=NULL) i2c_mutex->ReleaseMutex();*/
    169 }
    170 
    171 BlCtrlV2_x4_speed::~BlCtrlV2_x4_speed(void)
    172 {
    173     SafeStop();
    174     Join();
    175     delete main_tab;
    176 }
    177 
    178 void BlCtrlV2_x4_speed::UseDefaultPlot(void)
    179 {
    180     Tab *plot_tab=new Tab(tab,"Mesures");
    181         DataPlot1D *av_g_plot=new DataPlot1D(plot_tab->NewRow(),"avg",0,10000);
    182             av_g_plot->AddCurve(output->Element(0,0));
    183             av_g_plot->AddCurve(output->Element(0,1),DataPlot::Blue);
    184         DataPlot1D *av_d_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"avd",0,10000);
    185             av_d_plot->AddCurve(output->Element(2,0));
    186             av_d_plot->AddCurve(output->Element(2,1),DataPlot::Blue);
    187         DataPlot1D *ar_g_plot=new DataPlot1D(plot_tab->NewRow(),"arg",0,10000);
    188             ar_g_plot->AddCurve(output->Element(3,0));
    189             ar_g_plot->AddCurve(output->Element(3,1),DataPlot::Blue);
    190         DataPlot1D *ar_d_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"ard",0,10000);
    191             ar_d_plot->AddCurve(output->Element(1,0));
    192             ar_d_plot->AddCurve(output->Element(1,1),DataPlot::Blue);
    193 }
    194 
    195 float BlCtrlV2_x4_speed::TrimValue(void)
    196 {
    197     return (float)trim->Value();
    198 }
    199 
    200 int BlCtrlV2_x4_speed::StartValue(void)
    201 {
    202     return start_value->Value();
    203 }
    204 
    205 void BlCtrlV2_x4_speed::Run(void)
    206 {
    207     WarnUponSwitches(true);
    208 
    209     SetPeriodUS(TAU_US);
    210 
    211     while(!ToBeStopped())
    212     {
    213         WaitPeriod();
    214 
    215         Update();
    216     }
    217 
    218     WarnUponSwitches(false);
    219 }
    220 
    221 void BlCtrlV2_x4_speed::Update(void)
    222 {
    223     float u_roll,u_pitch,u_yaw,u_gaz;
    224     float trim_roll,trim_pitch,trim_yaw;
    225     float pwm[4];
    226     uint16_t pwm_moteur[4];
    227 
    228 
    229     //on prend une fois pour toute le mutex et on fait des accès directs
    230     input->GetMutex();
    231 
    232     u_roll=input->ValueNoMutex(0,0);
    233     u_pitch=input->ValueNoMutex(1,0);
    234     u_yaw=input->ValueNoMutex(2,0);
    235     u_gaz=input->ValueNoMutex(3,0)+input->ValueNoMutex(7,0)*input->ValueNoMutex(7,0);//ugaz+trim*trim
    236     trim_roll=input->ValueNoMutex(4,0);
    237     trim_pitch=input->ValueNoMutex(5,0);
    238     trim_yaw=input->ValueNoMutex(6,0);
    239 
    240     input->ReleaseMutex();
    241 
    242     if(pas->CurrentIndex()==1)
    243     {
    244         trim_yaw=-trim_yaw;
    245         u_yaw=-u_yaw;
    246     }
    247 
    248     //rt_printf("%f %f %f %f\n",u_roll,u_pitch,u_yaw,u_gaz);
    249     //if(u_gaz!=0) rt_printf("gaz: %f\n",u_gaz);
    250 
    251     //avant gauche
    252     if(u_gaz+u_pitch+u_roll+u_yaw>0)
    253     {
    254         pwm[0]=trim_pitch+trim_roll+trim_yaw+sqrtf(u_gaz+u_pitch+u_roll+u_yaw);
    255     }else
    256     {
    257         pwm[0]=trim_pitch+trim_roll+trim_yaw;
    258     }
    259 
    260     //arriere gauche
    261     if(u_gaz-u_pitch+u_roll-u_yaw>0)
    262     {
    263         pwm[3]=-trim_pitch+trim_roll-trim_yaw+sqrtf(u_gaz-u_pitch+u_roll-u_yaw);
    264     }else
    265     {
    266         pwm[3]=-trim_pitch+trim_roll-trim_yaw;
    267     }
    268 
    269     //arriere droit
    270     if(u_gaz-u_pitch-u_roll+u_yaw>0)
    271     {
    272         pwm[1]=-trim_pitch-trim_roll+trim_yaw+sqrtf(u_gaz-u_pitch-u_roll+u_yaw);
    273     }else
    274     {
    275         pwm[1]=-trim_pitch-trim_roll+trim_yaw;
    276     }
    277 
    278     //avant droit
    279     if(u_gaz+u_pitch-u_roll-u_yaw>0)
    280     {
    281         pwm[2]=trim_pitch-trim_roll-trim_yaw+sqrtf(u_gaz+u_pitch-u_roll-u_yaw);
    282     }else
    283     {
    284         pwm[2]=trim_pitch-trim_roll-trim_yaw;
    285     }
    286 
    287 
    288     int_av_g+=ki->Value()*(pwm[0]-speed_av_g);
    289     pwm[0]=kp->Value()*(pwm[0]-speed_av_g)+int_av_g;
    290 
    291     int_ar_g+=ki->Value()*(pwm[3]-speed_ar_g);
    292     pwm[3]=kp->Value()*(pwm[3]-speed_ar_g)+int_ar_g;
    293 
    294     int_ar_d+=ki->Value()*(pwm[1]-speed_ar_d);
    295     pwm[1]=kp->Value()*(pwm[1]-speed_ar_d)+int_ar_d;
    296 
    297     int_av_d+=ki->Value()*(pwm[2]-speed_av_d);
    298     pwm[2]=kp->Value()*(pwm[2]-speed_av_d)+int_av_d;
    299 
    300 //rt_printf("%f\n",pwm[0]);
    301     for(int i=0;i<4;i++) pwm_moteur[i]=SatPWM(pwm[i],min->Value(),max->Value());
    302 
    303     if(button_avg->Clicked()==true)
    304     {
    305         tested_motor=0;
    306         StartTest();
    307     }
    308     if(button_avd->Clicked()==true)
    309     {
    310         tested_motor=2;
    311         StartTest();
    312     }
    313     if(button_arg->Clicked()==true)
    314     {
    315         tested_motor=3;
    316         StartTest();
    317     }
    318     if(button_ard->Clicked()==true)
    319     {
    320         tested_motor=1;
    321         StartTest();
    322     }
    323 
    324     if(tested_motor!=-1)
    325     {
    326         for(int i=0;i<4;i++)
    327         {
    328             pwm_moteur[i]=0;
    329         }
    330         pwm_moteur[tested_motor]=(uint16_t)test->Value();
    331 
    332         if(GetTime()>(start_time+2*1000000000)) StopTest();
    333     }
    334 
    335 
    336     i2cport->GetMutex();
    337 
    338     if(enabled==true)
    339     {
    340         i2cport->SetSlave(slave_address+av_g->CurrentIndex());
    341         WriteValue(pwm_moteur[0]);
    342 
    343         i2cport->SetSlave(slave_address+av_d->CurrentIndex());
    344         WriteValue(pwm_moteur[2]);
    345 
    346         i2cport->SetSlave(slave_address+ar_g->CurrentIndex());
    347         WriteValue(pwm_moteur[3]);
    348 
    349         i2cport->SetSlave(slave_address+ar_d->CurrentIndex());
    350         WriteValue(pwm_moteur[1]);
    351 
    352     }
    353     else
    354     {
    355         for(int i=0;i<4;i++)
    356         {
    357             i2cport->SetSlave(slave_address+i);
    358             WriteValue(0);
    359         }
    360         int_av_g=0;
    361         int_av_d=0;
    362         int_ar_g=0;
    363         int_ar_d=0;
    364     }
    365 
    366     i2cport->SetSlave(slave_address+av_g->CurrentIndex());
    367     speed_av_g=GetSpeed();
    368 
    369     i2cport->SetSlave(slave_address+av_d->CurrentIndex());
    370     speed_av_d=GetSpeed();
    371 
    372     i2cport->SetSlave(slave_address+ar_g->CurrentIndex());
    373     speed_ar_g=GetSpeed();
    374 
    375     i2cport->SetSlave(slave_address+ar_d->CurrentIndex());
    376     speed_ar_d=GetSpeed();
    377 
    378     i2cport->ReleaseMutex();
    379 
    380 
    381     //on prend une fois pour toute le mutex et on fait des accès directs
    382     output->GetMutex();
    383     output->SetValueNoMutex(0,0,speed_av_g);
    384     output->SetValueNoMutex(1,0,speed_ar_d);
    385     output->SetValueNoMutex(2,0,speed_av_d);
    386     output->SetValueNoMutex(3,0,speed_ar_g);
    387    // rt_printf("%i %i %i %i\n",pwm_moteur[0],pwm_moteur[1],pwm_moteur[2],pwm_moteur[3]);
    388     output->ReleaseMutex();
    389 
    390     output->SetDataTime(GetTime());
    391     ProcessUpdate(output);
    392 
    393 }
    394 
    395 void BlCtrlV2_x4_speed::StartTest(void)
    396 {
    397     start_time=GetTime();
    398     SetEnabled(true);
    399 }
    400 
    401 void BlCtrlV2_x4_speed::StopTest(void)
    402 {
    403     SetEnabled(false);
    404     tested_motor=-1;
    405 }
    406 
    407 uint16_t BlCtrlV2_x4_speed::SatPWM(float vel_cons,uint16_t min,uint16_t max)
    408 {
    409   uint16_t sat_value=(uint16_t)vel_cons;
    410 
    411   if(vel_cons>((float)sat_value+0.5)) sat_value++;
    412 
    413   if(vel_cons<(float)min) sat_value=min;
    414   if(vel_cons>(float)max) sat_value=max;
    415 
    416   return sat_value;
    417 }
    418 
    419 void BlCtrlV2_x4_speed::LockUserInterface(void)
    420 {
    421     reglages_groupbox->setEnabled(false);
    422 }
    423 
    424 void BlCtrlV2_x4_speed::UnlockUserInterface(void)
    425 {
    426     reglages_groupbox->setEnabled(true);
    427 }
    428 
    429 void BlCtrlV2_x4_speed::SetEnabled(bool status)
    430 {
    431     enabled=status;
    432     if(enabled==true)
    433     {
    434         LockUserInterface();
    435 
    436         flight_start_time = GetTime();
    437     }
    438     else
    439     {
    440         UnlockUserInterface();
    441 
    442         Time now= GetTime();
    443         int t_sec;
    444         FILE *file;
    445         char ligne[32];
    446 
    447         t_sec=(now - flight_start_time)/1000000000;
    448         time_sec+=t_sec;
    449 
    450         Printf("temps de vol: %is = %imin\n",t_sec,t_sec/60);
    451         Printf("temps de vol total: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
    452 
    453         file=fopen("/etc/flight_time","w");
    454         if (file == NULL)
    455         {
    456             Thread::Err("Erreur a l'ouverture du fichier d'info vol\n");
    457         }
    458         else
    459         {
    460             sprintf(ligne,"%i",time_sec);
    461             fputs(ligne,file);
    462             fclose(file);
    463         }
    464     }
    465 }
    466 
    467 void BlCtrlV2_x4_speed::SetUroll(float value)
    468 {
    469     input->SetValue(0,0,value);
    470 }
    471 
    472 void BlCtrlV2_x4_speed::SetUpitch(float value)
    473 {
    474     input->SetValue(1,0,value);
    475 }
    476 
    477 void BlCtrlV2_x4_speed::SetUyaw(float value)
    478 {
    479     input->SetValue(2,0,value);
    480 }
    481 
    482 void BlCtrlV2_x4_speed::SetUgaz(float value)
    483 {
    484     input->SetValue(3,0,value);
    485 }
    486 
    487 void BlCtrlV2_x4_speed::SetRollTrim(float value)
    488 {
    489     input->SetValue(4,0,value);
    490 }
    491 
    492 void BlCtrlV2_x4_speed::SetPitchTrim(float value)
    493 {
    494     input->SetValue(5,0,value);
    495 }
    496 
    497 void BlCtrlV2_x4_speed::SetYawTrim(float value)
    498 {
    499     input->SetValue(6,0,value);
    500 }
    501 
    502 void BlCtrlV2_x4_speed::SetGazTrim(float value)
    503 {
    504     input->SetValue(7,0,value);
    505 }
    506 
    507 void BlCtrlV2_x4_speed::WriteValue(uint16_t value)
    508 {
    509     unsigned char tx[2];
    510     ssize_t written;
    511 
    512     tx[0]=(unsigned char)(value>>3);//msb
    513     tx[1]=16+8+(value&0x07);//16+8 pour recuperer la vitesse
    514     written = i2cport->Write(tx, 2);
    515     if(written<0)
    516     {
    517         Thread::Err("erreur rt_dev_write (%s)\n",strerror(-written));
    518     }
    519     else if (written != 2)
    520     {
    521         Thread::Err("erreur rt_dev_write %i/2\n",written);
    522     }
    523 }
    524 
    525 float BlCtrlV2_x4_speed::GetSpeed(void)
    526 {
    527     ssize_t read;
    528     uint8_t value;
    529     read = i2cport->Read(&value, 1);
    530 
    531     if(read<0)
    532     {
    533         Thread::Err("erreur rt_dev_read (%s)\n",strerror(-read));
    534     }
    535     else if (read != 1)
    536     {
    537         Thread::Err("erreur rt_dev_read %i/2\n",read);
    538 
    539     }
    540 
    541     return value*780./poles->Value();
     436  }
     437}
     438
     439void BlCtrlV2_x4_speed::SetUroll(float value) { input->SetValue(0, 0, value); }
     440
     441void BlCtrlV2_x4_speed::SetUpitch(float value) { input->SetValue(1, 0, value); }
     442
     443void BlCtrlV2_x4_speed::SetUyaw(float value) { input->SetValue(2, 0, value); }
     444
     445void BlCtrlV2_x4_speed::SetUgaz(float value) { input->SetValue(3, 0, value); }
     446
     447void BlCtrlV2_x4_speed::SetRollTrim(float value) {
     448  input->SetValue(4, 0, value);
     449}
     450
     451void BlCtrlV2_x4_speed::SetPitchTrim(float value) {
     452  input->SetValue(5, 0, value);
     453}
     454
     455void BlCtrlV2_x4_speed::SetYawTrim(float value) {
     456  input->SetValue(6, 0, value);
     457}
     458
     459void BlCtrlV2_x4_speed::SetGazTrim(float value) {
     460  input->SetValue(7, 0, value);
     461}
     462
     463void BlCtrlV2_x4_speed::WriteValue(uint16_t value) {
     464  unsigned char tx[2];
     465  ssize_t written;
     466
     467  tx[0] = (unsigned char)(value >> 3); // msb
     468  tx[1] = 16 + 8 + (value & 0x07);     // 16+8 pour recuperer la vitesse
     469  written = i2cport->Write(tx, 2);
     470  if (written < 0) {
     471    Thread::Err("erreur rt_dev_write (%s)\n", strerror(-written));
     472  } else if (written != 2) {
     473    Thread::Err("erreur rt_dev_write %i/2\n", written);
     474  }
     475}
     476
     477float BlCtrlV2_x4_speed::GetSpeed(void) {
     478  ssize_t read;
     479  uint8_t value;
     480  read = i2cport->Read(&value, 1);
     481
     482  if (read < 0) {
     483    Thread::Err("erreur rt_dev_read (%s)\n", strerror(-read));
     484  } else if (read != 1) {
     485    Thread::Err("erreur rt_dev_read %i/2\n", read);
     486  }
     487
     488  return value * 780. / poles->Value();
    542489}
    543490
Note: See TracChangeset for help on using the changeset viewer.