Changeset 15 in flair-src for trunk/lib/FlairSensorActuator/src/BlCtrlV2_x4_speed.cpp

Timestamp:

Apr 8, 2016, 3:40:57 PM (9 years ago)

Author:

Bayard Gildas

Message:

sources reformatted with flair-format-dir script

File:

• trunk/lib/FlairSensorActuator/src/BlCtrlV2_x4_speed.cpp (modified) (1 diff)

trunk/lib/FlairSensorActuator/src/BlCtrlV2_x4_speed.cpp

@@ -38 +38 @@
 using namespace flair::gui;
 
-namespace flair
-{
-namespace actuator
-{
-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)
-{
-    this->i2cport=i2cport;
-        slave_address=base_address;
-        tested_motor=-1;
-        enabled=false;
-        int_av_g=0;
-        int_av_d=0;
-        int_ar_g=0;
-        int_ar_d=0;
-
-        //flight time
-        FILE *file;
-    file=fopen("/etc/flight_time","r");
-    if (file == NULL)
-    {
-        Printf("fichier d'info de vol vide\n");
-        time_sec=0;
+namespace flair {
+namespace actuator {
+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) {
+  this->i2cport = i2cport;
+  slave_address = base_address;
+  tested_motor = -1;
+  enabled = false;
+  int_av_g = 0;
+  int_av_d = 0;
+  int_ar_g = 0;
+  int_ar_d = 0;
+
+  // flight time
+  FILE *file;
+  file = fopen("/etc/flight_time", "r");
+  if (file == NULL) {
+    Printf("fichier d'info de vol vide\n");
+    time_sec = 0;
+  } else {
+    char ligne[32];
+    fgets(ligne, 32, file);
+    time_sec = atoi(ligne);
+    Printf("temps de vol total: %is = %imin = %ih\n", time_sec, time_sec / 60,
+           time_sec / 3600);
+    fclose(file);
+  }
+
+  // station sol
+  main_tab = new Tab(parent->GetTabWidget(), name);
+  tab = new TabWidget(main_tab->NewRow(), name);
+  Tab *sensor_tab = new Tab(tab, "Reglages");
+  reglages_groupbox = new GroupBox(sensor_tab->NewRow(), name);
+  poles = new SpinBox(reglages_groupbox->NewRow(), "nb poles", 0, 255, 1);
+  kp = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "kp", 0., 255,
+                         0.001, 4);
+  ki = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "ki", 0., 255,
+                         0.001, 4);
+  min = new SpinBox(reglages_groupbox->NewRow(), "min pwm", 0., 2048, 1);
+  max =
+      new SpinBox(reglages_groupbox->LastRowLastCol(), "max pwm", 0., 2048, 1);
+  test = new SpinBox(reglages_groupbox->LastRowLastCol(), "test value", 0.,
+                     2048, 1);
+  start_value = new SpinBox(reglages_groupbox->NewRow(), "valeur demarrage", 0,
+                            10000, 10);
+  trim = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(), "pas decollage",
+                           0, 1000, .1);
+
+  av_g = new ComboBox(reglages_groupbox->NewRow(), "avant gauche");
+  av_g->AddItem("1");
+  av_g->AddItem("2");
+  av_g->AddItem("3");
+  av_g->AddItem("4");
+  button_avg = new PushButton(reglages_groupbox->LastRowLastCol(), "test avg");
+
+  av_d = new ComboBox(reglages_groupbox->LastRowLastCol(), "avant droite:");
+  av_d->AddItem("1");
+  av_d->AddItem("2");
+  av_d->AddItem("3");
+  av_d->AddItem("4");
+  button_avd = new PushButton(reglages_groupbox->LastRowLastCol(), "test avd");
+
+  ar_g = new ComboBox(reglages_groupbox->NewRow(), "arriere gauche:");
+  ar_g->AddItem("1");
+  ar_g->AddItem("2");
+  ar_g->AddItem("3");
+  ar_g->AddItem("4");
+  button_arg = new PushButton(reglages_groupbox->LastRowLastCol(), "test arg");
+
+  ar_d = new ComboBox(reglages_groupbox->LastRowLastCol(), "arriere droite:");
+  ar_d->AddItem("1");
+  ar_d->AddItem("2");
+  ar_d->AddItem("3");
+  ar_d->AddItem("4");
+  button_ard = new PushButton(reglages_groupbox->LastRowLastCol(), "test ard");
+
+  pas = new ComboBox(reglages_groupbox->NewRow(), "pas helice avant gauche:");
+  pas->AddItem("normal");
+  pas->AddItem("inverse");
+
+  input = new cvmatrix((IODevice *)this, 8, 1, floatType);
+
+  cvmatrix_descriptor *desc = new cvmatrix_descriptor(4, 2);
+  desc->SetElementName(0, 0, "avant gauche");
+  desc->SetElementName(1, 0, "arriere droite");
+  desc->SetElementName(2, 0, "avant droite");
+  desc->SetElementName(3, 0, "arriere gauche");
+
+  desc->SetElementName(0, 1, "cons avant gauche");
+  desc->SetElementName(1, 1, "cons arriere droite");
+  desc->SetElementName(2, 1, "cons avant droite");
+  desc->SetElementName(3, 1, "cons arriere gauche");
+  output = new cvmatrix((IODevice *)this, desc, floatType);
+
+  /*
+
+  //le 3ieme lu est la tension batteire
+          if(i2c_mutex!=NULL) i2c_mutex->GetMutex();
+      uint16_t pwm_moteur;
+      pwm_moteur=0;
+      ssize_t read;
+      uint8_t rx[8];
+      SetSlave(slave_address);
+
+      for(int j=0;j<10;j++)
+      {
+
+
+          WriteValue(pwm_moteur);
+
+
+          read = rt_dev_read(i2c_fd, rx, sizeof(rx));
+
+          if(read<0)
+          {
+              rt_printf("BlCtrlV2_x4_speed::BlCtrlV2_x4_speed: %s, erreur
+  rt_dev_read (%s)\n",IODevice::ObjectName().c_str(),strerror(-read));
+          }
+          else if (read != sizeof(rx))
+          {
+              rt_printf("BlCtrlV2_x4_speed::BlCtrlV2_x4_speed: %s, erreur
+  rt_dev_read %i/2\n",IODevice::ObjectName().c_str(),read);
+
+          }
+          for(int i=0;i<sizeof(rx);i++) printf("%i ",rx[i]);
+
+          printf("\n");
+
+      }
+
+      if(i2c_mutex!=NULL) i2c_mutex->ReleaseMutex();*/
+}
+
+BlCtrlV2_x4_speed::~BlCtrlV2_x4_speed(void) {
+  SafeStop();
+  Join();
+  delete main_tab;
+}
+
+void BlCtrlV2_x4_speed::UseDefaultPlot(void) {
+  Tab *plot_tab = new Tab(tab, "Mesures");
+  DataPlot1D *av_g_plot = new DataPlot1D(plot_tab->NewRow(), "avg", 0, 10000);
+  av_g_plot->AddCurve(output->Element(0, 0));
+  av_g_plot->AddCurve(output->Element(0, 1), DataPlot::Blue);
+  DataPlot1D *av_d_plot =
+      new DataPlot1D(plot_tab->LastRowLastCol(), "avd", 0, 10000);
+  av_d_plot->AddCurve(output->Element(2, 0));
+  av_d_plot->AddCurve(output->Element(2, 1), DataPlot::Blue);
+  DataPlot1D *ar_g_plot = new DataPlot1D(plot_tab->NewRow(), "arg", 0, 10000);
+  ar_g_plot->AddCurve(output->Element(3, 0));
+  ar_g_plot->AddCurve(output->Element(3, 1), DataPlot::Blue);
+  DataPlot1D *ar_d_plot =
+      new DataPlot1D(plot_tab->LastRowLastCol(), "ard", 0, 10000);
+  ar_d_plot->AddCurve(output->Element(1, 0));
+  ar_d_plot->AddCurve(output->Element(1, 1), DataPlot::Blue);
+}
+
+float BlCtrlV2_x4_speed::TrimValue(void) { return (float)trim->Value(); }
+
+int BlCtrlV2_x4_speed::StartValue(void) { return start_value->Value(); }
+
+void BlCtrlV2_x4_speed::Run(void) {
+  WarnUponSwitches(true);
+
+  SetPeriodUS(TAU_US);
+
+  while (!ToBeStopped()) {
+    WaitPeriod();
+
+    Update();
+  }
+
+  WarnUponSwitches(false);
+}
+
+void BlCtrlV2_x4_speed::Update(void) {
+  float u_roll, u_pitch, u_yaw, u_gaz;
+  float trim_roll, trim_pitch, trim_yaw;
+  float pwm[4];
+  uint16_t pwm_moteur[4];
+
+  // on prend une fois pour toute le mutex et on fait des accès directs
+  input->GetMutex();
+
+  u_roll = input->ValueNoMutex(0, 0);
+  u_pitch = input->ValueNoMutex(1, 0);
+  u_yaw = input->ValueNoMutex(2, 0);
+  u_gaz =
+      input->ValueNoMutex(3, 0) +
+      input->ValueNoMutex(7, 0) * input->ValueNoMutex(7, 0); // ugaz+trim*trim
+  trim_roll = input->ValueNoMutex(4, 0);
+  trim_pitch = input->ValueNoMutex(5, 0);
+  trim_yaw = input->ValueNoMutex(6, 0);
+
+  input->ReleaseMutex();
+
+  if (pas->CurrentIndex() == 1) {
+    trim_yaw = -trim_yaw;
+    u_yaw = -u_yaw;
+  }
+
+  // rt_printf("%f %f %f %f\n",u_roll,u_pitch,u_yaw,u_gaz);
+  // if(u_gaz!=0) rt_printf("gaz: %f\n",u_gaz);
+
+  // avant gauche
+  if (u_gaz + u_pitch + u_roll + u_yaw > 0) {
+    pwm[0] = trim_pitch + trim_roll + trim_yaw +
+             sqrtf(u_gaz + u_pitch + u_roll + u_yaw);
+  } else {
+    pwm[0] = trim_pitch + trim_roll + trim_yaw;
+  }
+
+  // arriere gauche
+  if (u_gaz - u_pitch + u_roll - u_yaw > 0) {
+    pwm[3] = -trim_pitch + trim_roll - trim_yaw +
+             sqrtf(u_gaz - u_pitch + u_roll - u_yaw);
+  } else {
+    pwm[3] = -trim_pitch + trim_roll - trim_yaw;
+  }
+
+  // arriere droit
+  if (u_gaz - u_pitch - u_roll + u_yaw > 0) {
+    pwm[1] = -trim_pitch - trim_roll + trim_yaw +
+             sqrtf(u_gaz - u_pitch - u_roll + u_yaw);
+  } else {
+    pwm[1] = -trim_pitch - trim_roll + trim_yaw;
+  }
+
+  // avant droit
+  if (u_gaz + u_pitch - u_roll - u_yaw > 0) {
+    pwm[2] = trim_pitch - trim_roll - trim_yaw +
+             sqrtf(u_gaz + u_pitch - u_roll - u_yaw);
+  } else {
+    pwm[2] = trim_pitch - trim_roll - trim_yaw;
+  }
+
+  int_av_g += ki->Value() * (pwm[0] - speed_av_g);
+  pwm[0] = kp->Value() * (pwm[0] - speed_av_g) + int_av_g;
+
+  int_ar_g += ki->Value() * (pwm[3] - speed_ar_g);
+  pwm[3] = kp->Value() * (pwm[3] - speed_ar_g) + int_ar_g;
+
+  int_ar_d += ki->Value() * (pwm[1] - speed_ar_d);
+  pwm[1] = kp->Value() * (pwm[1] - speed_ar_d) + int_ar_d;
+
+  int_av_d += ki->Value() * (pwm[2] - speed_av_d);
+  pwm[2] = kp->Value() * (pwm[2] - speed_av_d) + int_av_d;
+
+  // rt_printf("%f\n",pwm[0]);
+  for (int i = 0; i < 4; i++)
+    pwm_moteur[i] = SatPWM(pwm[i], min->Value(), max->Value());
+
+  if (button_avg->Clicked() == true) {
+    tested_motor = 0;
+    StartTest();
+  }
+  if (button_avd->Clicked() == true) {
+    tested_motor = 2;
+    StartTest();
+  }
+  if (button_arg->Clicked() == true) {
+    tested_motor = 3;
+    StartTest();
+  }
+  if (button_ard->Clicked() == true) {
+    tested_motor = 1;
+    StartTest();
+  }
+
+  if (tested_motor != -1) {
+    for (int i = 0; i < 4; i++) {
+      pwm_moteur[i] = 0;
     }
-    else
-    {
-        char ligne[32];
-        fgets(ligne, 32, file);
-        time_sec=atoi(ligne);
-        Printf("temps de vol total: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
-        fclose(file);
+    pwm_moteur[tested_motor] = (uint16_t)test->Value();
+
+    if (GetTime() > (start_time + 2 * 1000000000))
+      StopTest();
+  }
+
+  i2cport->GetMutex();
+
+  if (enabled == true) {
+    i2cport->SetSlave(slave_address + av_g->CurrentIndex());
+    WriteValue(pwm_moteur[0]);
+
+    i2cport->SetSlave(slave_address + av_d->CurrentIndex());
+    WriteValue(pwm_moteur[2]);
+
+    i2cport->SetSlave(slave_address + ar_g->CurrentIndex());
+    WriteValue(pwm_moteur[3]);
+
+    i2cport->SetSlave(slave_address + ar_d->CurrentIndex());
+    WriteValue(pwm_moteur[1]);
+
+  } else {
+    for (int i = 0; i < 4; i++) {
+      i2cport->SetSlave(slave_address + i);
+      WriteValue(0);
     }
-
-    //station sol
-    main_tab=new Tab(parent->GetTabWidget(),name);
-    tab=new TabWidget(main_tab->NewRow(),name);
-        Tab *sensor_tab=new Tab(tab,"Reglages");
-        reglages_groupbox=new GroupBox(sensor_tab->NewRow(),name);
-            poles=new SpinBox(reglages_groupbox->NewRow(),"nb poles",0,255,1);
-            kp=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"kp",0.,255,0.001,4);
-            ki=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"ki",0.,255,0.001,4);
-            min=new SpinBox(reglages_groupbox->NewRow(),"min pwm",0.,2048,1);
-            max=new SpinBox(reglages_groupbox->LastRowLastCol(),"max pwm",0.,2048,1);
-            test=new SpinBox(reglages_groupbox->LastRowLastCol(),"test value",0.,2048,1);
-            start_value=new SpinBox(reglages_groupbox->NewRow(),"valeur demarrage",0,10000,10);
-            trim=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"pas decollage",0,1000,.1);
-
-            av_g=new ComboBox(reglages_groupbox->NewRow(),"avant gauche");
-            av_g->AddItem("1");
-            av_g->AddItem("2");
-            av_g->AddItem("3");
-            av_g->AddItem("4");
-            button_avg=new PushButton(reglages_groupbox->LastRowLastCol(),"test avg");
-
-            av_d=new ComboBox(reglages_groupbox->LastRowLastCol(),"avant droite:");
-            av_d->AddItem("1");
-            av_d->AddItem("2");
-            av_d->AddItem("3");
-            av_d->AddItem("4");
-            button_avd=new PushButton(reglages_groupbox->LastRowLastCol(),"test avd");
-
-            ar_g=new ComboBox(reglages_groupbox->NewRow(),"arriere gauche:");
-            ar_g->AddItem("1");
-            ar_g->AddItem("2");
-            ar_g->AddItem("3");
-            ar_g->AddItem("4");
-            button_arg=new PushButton(reglages_groupbox->LastRowLastCol(),"test arg");
-
-            ar_d=new ComboBox(reglages_groupbox->LastRowLastCol(),"arriere droite:");
-            ar_d->AddItem("1");
-            ar_d->AddItem("2");
-            ar_d->AddItem("3");
-            ar_d->AddItem("4");
-            button_ard=new PushButton(reglages_groupbox->LastRowLastCol(),"test ard");
-
-            pas=new ComboBox(reglages_groupbox->NewRow(),"pas helice avant gauche:");
-            pas->AddItem("normal");
-            pas->AddItem("inverse");
-
-    input=new cvmatrix((IODevice*)this,8,1,floatType);
-
-    cvmatrix_descriptor* desc=new cvmatrix_descriptor(4,2);
-    desc->SetElementName(0,0,"avant gauche");
-        desc->SetElementName(1,0,"arriere droite");
-        desc->SetElementName(2,0,"avant droite");
-        desc->SetElementName(3,0,"arriere gauche");
-
-        desc->SetElementName(0,1,"cons avant gauche");
-        desc->SetElementName(1,1,"cons arriere droite");
-        desc->SetElementName(2,1,"cons avant droite");
-        desc->SetElementName(3,1,"cons arriere gauche");
-    output=new cvmatrix((IODevice*)this,desc,floatType);
-
-
-
-/*
-
-//le 3ieme lu est la tension batteire
-        if(i2c_mutex!=NULL) i2c_mutex->GetMutex();
-    uint16_t pwm_moteur;
-    pwm_moteur=0;
-    ssize_t read;
-    uint8_t rx[8];
-    SetSlave(slave_address);
-
-    for(int j=0;j<10;j++)
-    {
-
-
-        WriteValue(pwm_moteur);
-
-
-        read = rt_dev_read(i2c_fd, rx, sizeof(rx));
-
-        if(read<0)
-        {
-            rt_printf("BlCtrlV2_x4_speed::BlCtrlV2_x4_speed: %s, erreur rt_dev_read (%s)\n",IODevice::ObjectName().c_str(),strerror(-read));
-        }
-        else if (read != sizeof(rx))
-        {
-            rt_printf("BlCtrlV2_x4_speed::BlCtrlV2_x4_speed: %s, erreur rt_dev_read %i/2\n",IODevice::ObjectName().c_str(),read);
-
-        }
-        for(int i=0;i<sizeof(rx);i++) printf("%i ",rx[i]);
-
-        printf("\n");
-
+    int_av_g = 0;
+    int_av_d = 0;
+    int_ar_g = 0;
+    int_ar_d = 0;
+  }
+
+  i2cport->SetSlave(slave_address + av_g->CurrentIndex());
+  speed_av_g = GetSpeed();
+
+  i2cport->SetSlave(slave_address + av_d->CurrentIndex());
+  speed_av_d = GetSpeed();
+
+  i2cport->SetSlave(slave_address + ar_g->CurrentIndex());
+  speed_ar_g = GetSpeed();
+
+  i2cport->SetSlave(slave_address + ar_d->CurrentIndex());
+  speed_ar_d = GetSpeed();
+
+  i2cport->ReleaseMutex();
+
+  // on prend une fois pour toute le mutex et on fait des accès directs
+  output->GetMutex();
+  output->SetValueNoMutex(0, 0, speed_av_g);
+  output->SetValueNoMutex(1, 0, speed_ar_d);
+  output->SetValueNoMutex(2, 0, speed_av_d);
+  output->SetValueNoMutex(3, 0, speed_ar_g);
+  // rt_printf("%i %i %i
+  // %i\n",pwm_moteur[0],pwm_moteur[1],pwm_moteur[2],pwm_moteur[3]);
+  output->ReleaseMutex();
+
+  output->SetDataTime(GetTime());
+  ProcessUpdate(output);
+}
+
+void BlCtrlV2_x4_speed::StartTest(void) {
+  start_time = GetTime();
+  SetEnabled(true);
+}
+
+void BlCtrlV2_x4_speed::StopTest(void) {
+  SetEnabled(false);
+  tested_motor = -1;
+}
+
+uint16_t BlCtrlV2_x4_speed::SatPWM(float vel_cons, uint16_t min, uint16_t max) {
+  uint16_t sat_value = (uint16_t)vel_cons;
+
+  if (vel_cons > ((float)sat_value + 0.5))
+    sat_value++;
+
+  if (vel_cons < (float)min)
+    sat_value = min;
+  if (vel_cons > (float)max)
+    sat_value = max;
+
+  return sat_value;
+}
+
+void BlCtrlV2_x4_speed::LockUserInterface(void) {
+  reglages_groupbox->setEnabled(false);
+}
+
+void BlCtrlV2_x4_speed::UnlockUserInterface(void) {
+  reglages_groupbox->setEnabled(true);
+}
+
+void BlCtrlV2_x4_speed::SetEnabled(bool status) {
+  enabled = status;
+  if (enabled == true) {
+    LockUserInterface();
+
+    flight_start_time = GetTime();
+  } else {
+    UnlockUserInterface();
+
+    Time now = GetTime();
+    int t_sec;
+    FILE *file;
+    char ligne[32];
+
+    t_sec = (now - flight_start_time) / 1000000000;
+    time_sec += t_sec;
+
+    Printf("temps de vol: %is = %imin\n", t_sec, t_sec / 60);
+    Printf("temps de vol total: %is = %imin = %ih\n", time_sec, time_sec / 60,
+           time_sec / 3600);
+
+    file = fopen("/etc/flight_time", "w");
+    if (file == NULL) {
+      Thread::Err("Erreur a l'ouverture du fichier d'info vol\n");
+    } else {
+      sprintf(ligne, "%i", time_sec);
+      fputs(ligne, file);
+      fclose(file);
     }
-
-    if(i2c_mutex!=NULL) i2c_mutex->ReleaseMutex();*/
-}
-
-BlCtrlV2_x4_speed::~BlCtrlV2_x4_speed(void)
-{
-    SafeStop();
-    Join();
-    delete main_tab;
-}
-
-void BlCtrlV2_x4_speed::UseDefaultPlot(void)
-{
-    Tab *plot_tab=new Tab(tab,"Mesures");
-        DataPlot1D *av_g_plot=new DataPlot1D(plot_tab->NewRow(),"avg",0,10000);
-            av_g_plot->AddCurve(output->Element(0,0));
-            av_g_plot->AddCurve(output->Element(0,1),DataPlot::Blue);
-        DataPlot1D *av_d_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"avd",0,10000);
-            av_d_plot->AddCurve(output->Element(2,0));
-            av_d_plot->AddCurve(output->Element(2,1),DataPlot::Blue);
-        DataPlot1D *ar_g_plot=new DataPlot1D(plot_tab->NewRow(),"arg",0,10000);
-            ar_g_plot->AddCurve(output->Element(3,0));
-            ar_g_plot->AddCurve(output->Element(3,1),DataPlot::Blue);
-        DataPlot1D *ar_d_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"ard",0,10000);
-            ar_d_plot->AddCurve(output->Element(1,0));
-            ar_d_plot->AddCurve(output->Element(1,1),DataPlot::Blue);
-}
-
-float BlCtrlV2_x4_speed::TrimValue(void)
-{
-    return (float)trim->Value();
-}
-
-int BlCtrlV2_x4_speed::StartValue(void)
-{
-    return start_value->Value();
-}
-
-void BlCtrlV2_x4_speed::Run(void)
-{
-    WarnUponSwitches(true);
-
-    SetPeriodUS(TAU_US);
-
-    while(!ToBeStopped())
-    {
-        WaitPeriod();
-
-        Update();
-    }
-
-    WarnUponSwitches(false);
-}
-
-void BlCtrlV2_x4_speed::Update(void)
-{
-    float u_roll,u_pitch,u_yaw,u_gaz;
-    float trim_roll,trim_pitch,trim_yaw;
-    float pwm[4];
-    uint16_t pwm_moteur[4];
-
-
-    //on prend une fois pour toute le mutex et on fait des accès directs
-    input->GetMutex();
-
-    u_roll=input->ValueNoMutex(0,0);
-    u_pitch=input->ValueNoMutex(1,0);
-    u_yaw=input->ValueNoMutex(2,0);
-    u_gaz=input->ValueNoMutex(3,0)+input->ValueNoMutex(7,0)*input->ValueNoMutex(7,0);//ugaz+trim*trim
-    trim_roll=input->ValueNoMutex(4,0);
-    trim_pitch=input->ValueNoMutex(5,0);
-    trim_yaw=input->ValueNoMutex(6,0);
-
-    input->ReleaseMutex();
-
-    if(pas->CurrentIndex()==1)
-    {
-        trim_yaw=-trim_yaw;
-        u_yaw=-u_yaw;
-    }
-
-    //rt_printf("%f %f %f %f\n",u_roll,u_pitch,u_yaw,u_gaz);
-    //if(u_gaz!=0) rt_printf("gaz: %f\n",u_gaz);
-
-    //avant gauche
-    if(u_gaz+u_pitch+u_roll+u_yaw>0)
-    {
-        pwm[0]=trim_pitch+trim_roll+trim_yaw+sqrtf(u_gaz+u_pitch+u_roll+u_yaw);
-    }else
-    {
-        pwm[0]=trim_pitch+trim_roll+trim_yaw;
-    }
-
-    //arriere gauche
-    if(u_gaz-u_pitch+u_roll-u_yaw>0)
-    {
-        pwm[3]=-trim_pitch+trim_roll-trim_yaw+sqrtf(u_gaz-u_pitch+u_roll-u_yaw);
-    }else
-    {
-        pwm[3]=-trim_pitch+trim_roll-trim_yaw;
-    }
-
-    //arriere droit
-    if(u_gaz-u_pitch-u_roll+u_yaw>0)
-    {
-        pwm[1]=-trim_pitch-trim_roll+trim_yaw+sqrtf(u_gaz-u_pitch-u_roll+u_yaw);
-    }else
-    {
-        pwm[1]=-trim_pitch-trim_roll+trim_yaw;
-    }
-
-    //avant droit
-    if(u_gaz+u_pitch-u_roll-u_yaw>0)
-    {
-        pwm[2]=trim_pitch-trim_roll-trim_yaw+sqrtf(u_gaz+u_pitch-u_roll-u_yaw);
-    }else
-    {
-        pwm[2]=trim_pitch-trim_roll-trim_yaw;
-    }
-
-
-    int_av_g+=ki->Value()*(pwm[0]-speed_av_g);
-    pwm[0]=kp->Value()*(pwm[0]-speed_av_g)+int_av_g;
-
-    int_ar_g+=ki->Value()*(pwm[3]-speed_ar_g);
-    pwm[3]=kp->Value()*(pwm[3]-speed_ar_g)+int_ar_g;
-
-    int_ar_d+=ki->Value()*(pwm[1]-speed_ar_d);
-    pwm[1]=kp->Value()*(pwm[1]-speed_ar_d)+int_ar_d;
-
-    int_av_d+=ki->Value()*(pwm[2]-speed_av_d);
-    pwm[2]=kp->Value()*(pwm[2]-speed_av_d)+int_av_d;
-
-//rt_printf("%f\n",pwm[0]);
-    for(int i=0;i<4;i++) pwm_moteur[i]=SatPWM(pwm[i],min->Value(),max->Value());
-
-    if(button_avg->Clicked()==true)
-    {
-        tested_motor=0;
-        StartTest();
-    }
-    if(button_avd->Clicked()==true)
-    {
-        tested_motor=2;
-        StartTest();
-    }
-    if(button_arg->Clicked()==true)
-    {
-        tested_motor=3;
-        StartTest();
-    }
-    if(button_ard->Clicked()==true)
-    {
-        tested_motor=1;
-        StartTest();
-    }
-
-    if(tested_motor!=-1)
-    {
-        for(int i=0;i<4;i++)
-        {
-            pwm_moteur[i]=0;
-        }
-        pwm_moteur[tested_motor]=(uint16_t)test->Value();
-
-        if(GetTime()>(start_time+2*1000000000)) StopTest();
-    }
-
-
-    i2cport->GetMutex();
-
-    if(enabled==true)
-    {
-        i2cport->SetSlave(slave_address+av_g->CurrentIndex());
-        WriteValue(pwm_moteur[0]);
-
-        i2cport->SetSlave(slave_address+av_d->CurrentIndex());
-        WriteValue(pwm_moteur[2]);
-
-        i2cport->SetSlave(slave_address+ar_g->CurrentIndex());
-        WriteValue(pwm_moteur[3]);
-
-        i2cport->SetSlave(slave_address+ar_d->CurrentIndex());
-        WriteValue(pwm_moteur[1]);
-
-    }
-    else
-    {
-        for(int i=0;i<4;i++)
-        {
-            i2cport->SetSlave(slave_address+i);
-            WriteValue(0);
-        }
-        int_av_g=0;
-        int_av_d=0;
-        int_ar_g=0;
-        int_ar_d=0;
-    }
-
-    i2cport->SetSlave(slave_address+av_g->CurrentIndex());
-    speed_av_g=GetSpeed();
-
-    i2cport->SetSlave(slave_address+av_d->CurrentIndex());
-    speed_av_d=GetSpeed();
-
-    i2cport->SetSlave(slave_address+ar_g->CurrentIndex());
-    speed_ar_g=GetSpeed();
-
-    i2cport->SetSlave(slave_address+ar_d->CurrentIndex());
-    speed_ar_d=GetSpeed();
-
-    i2cport->ReleaseMutex();
-
-
-    //on prend une fois pour toute le mutex et on fait des accès directs
-    output->GetMutex();
-    output->SetValueNoMutex(0,0,speed_av_g);
-    output->SetValueNoMutex(1,0,speed_ar_d);
-    output->SetValueNoMutex(2,0,speed_av_d);
-    output->SetValueNoMutex(3,0,speed_ar_g);
-   // rt_printf("%i %i %i %i\n",pwm_moteur[0],pwm_moteur[1],pwm_moteur[2],pwm_moteur[3]);
-    output->ReleaseMutex();
-
-    output->SetDataTime(GetTime());
-    ProcessUpdate(output);
-
-}
-
-void BlCtrlV2_x4_speed::StartTest(void)
-{
-    start_time=GetTime();
-    SetEnabled(true);
-}
-
-void BlCtrlV2_x4_speed::StopTest(void)
-{
-    SetEnabled(false);
-    tested_motor=-1;
-}
-
-uint16_t BlCtrlV2_x4_speed::SatPWM(float vel_cons,uint16_t min,uint16_t max)
-{
-  uint16_t sat_value=(uint16_t)vel_cons;
-
-  if(vel_cons>((float)sat_value+0.5)) sat_value++;
-
-  if(vel_cons<(float)min) sat_value=min;
-  if(vel_cons>(float)max) sat_value=max;
-
-  return sat_value;
-}
-
-void BlCtrlV2_x4_speed::LockUserInterface(void)
-{
-    reglages_groupbox->setEnabled(false);
-}
-
-void BlCtrlV2_x4_speed::UnlockUserInterface(void)
-{
-    reglages_groupbox->setEnabled(true);
-}
-
-void BlCtrlV2_x4_speed::SetEnabled(bool status)
-{
-    enabled=status;
-    if(enabled==true)
-    {
-        LockUserInterface();
-
-        flight_start_time = GetTime();
-    }
-    else
-    {
-        UnlockUserInterface();
-
-        Time now= GetTime();
-        int t_sec;
-        FILE *file;
-        char ligne[32];
-
-        t_sec=(now - flight_start_time)/1000000000;
-        time_sec+=t_sec;
-
-        Printf("temps de vol: %is = %imin\n",t_sec,t_sec/60);
-        Printf("temps de vol total: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
-
-        file=fopen("/etc/flight_time","w");
-        if (file == NULL)
-        {
-            Thread::Err("Erreur a l'ouverture du fichier d'info vol\n");
-        }
-        else
-        {
-            sprintf(ligne,"%i",time_sec);
-            fputs(ligne,file);
-            fclose(file);
-        }
-    }
-}
-
-void BlCtrlV2_x4_speed::SetUroll(float value)
-{
-    input->SetValue(0,0,value);
-}
-
-void BlCtrlV2_x4_speed::SetUpitch(float value)
-{
-    input->SetValue(1,0,value);
-}
-
-void BlCtrlV2_x4_speed::SetUyaw(float value)
-{
-    input->SetValue(2,0,value);
-}
-
-void BlCtrlV2_x4_speed::SetUgaz(float value)
-{
-    input->SetValue(3,0,value);
-}
-
-void BlCtrlV2_x4_speed::SetRollTrim(float value)
-{
-    input->SetValue(4,0,value);
-}
-
-void BlCtrlV2_x4_speed::SetPitchTrim(float value)
-{
-    input->SetValue(5,0,value);
-}
-
-void BlCtrlV2_x4_speed::SetYawTrim(float value)
-{
-    input->SetValue(6,0,value);
-}
-
-void BlCtrlV2_x4_speed::SetGazTrim(float value)
-{
-    input->SetValue(7,0,value);
-}
-
-void BlCtrlV2_x4_speed::WriteValue(uint16_t value)
-{
-    unsigned char tx[2];
-    ssize_t written;
-
-    tx[0]=(unsigned char)(value>>3);//msb
-    tx[1]=16+8+(value&0x07);//16+8 pour recuperer la vitesse
-    written = i2cport->Write(tx, 2);
-    if(written<0)
-    {
-        Thread::Err("erreur rt_dev_write (%s)\n",strerror(-written));
-    }
-    else if (written != 2)
-    {
-        Thread::Err("erreur rt_dev_write %i/2\n",written);
-    }
-}
-
-float BlCtrlV2_x4_speed::GetSpeed(void)
-{
-    ssize_t read;
-    uint8_t value;
-    read = i2cport->Read(&value, 1);
-
-    if(read<0)
-    {
-        Thread::Err("erreur rt_dev_read (%s)\n",strerror(-read));
-    }
-    else if (read != 1)
-    {
-        Thread::Err("erreur rt_dev_read %i/2\n",read);
-
-    }
-
-    return value*780./poles->Value();
+  }
+}
+
+void BlCtrlV2_x4_speed::SetUroll(float value) { input->SetValue(0, 0, value); }
+
+void BlCtrlV2_x4_speed::SetUpitch(float value) { input->SetValue(1, 0, value); }
+
+void BlCtrlV2_x4_speed::SetUyaw(float value) { input->SetValue(2, 0, value); }
+
+void BlCtrlV2_x4_speed::SetUgaz(float value) { input->SetValue(3, 0, value); }
+
+void BlCtrlV2_x4_speed::SetRollTrim(float value) {
+  input->SetValue(4, 0, value);
+}
+
+void BlCtrlV2_x4_speed::SetPitchTrim(float value) {
+  input->SetValue(5, 0, value);
+}
+
+void BlCtrlV2_x4_speed::SetYawTrim(float value) {
+  input->SetValue(6, 0, value);
+}
+
+void BlCtrlV2_x4_speed::SetGazTrim(float value) {
+  input->SetValue(7, 0, value);
+}
+
+void BlCtrlV2_x4_speed::WriteValue(uint16_t value) {
+  unsigned char tx[2];
+  ssize_t written;
+
+  tx[0] = (unsigned char)(value >> 3); // msb
+  tx[1] = 16 + 8 + (value & 0x07);     // 16+8 pour recuperer la vitesse
+  written = i2cport->Write(tx, 2);
+  if (written < 0) {
+    Thread::Err("erreur rt_dev_write (%s)\n", strerror(-written));
+  } else if (written != 2) {
+    Thread::Err("erreur rt_dev_write %i/2\n", written);
+  }
+}
+
+float BlCtrlV2_x4_speed::GetSpeed(void) {
+  ssize_t read;
+  uint8_t value;
+  read = i2cport->Read(&value, 1);
+
+  if (read < 0) {
+    Thread::Err("erreur rt_dev_read (%s)\n", strerror(-read));
+  } else if (read != 1) {
+    Thread::Err("erreur rt_dev_read %i/2\n", read);
+  }
+
+  return value * 780. / poles->Value();
 }