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


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

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairSensorActuator/src/Srf08.cpp

    r3 r15  
    3030using namespace flair::gui;
    3131
    32 namespace flair
    33 {
    34 namespace sensor
    35 {
    36 
    37 Srf08::Srf08(const FrameworkManager* parent,string name,I2cPort* i2cport,uint16_t address,uint8_t priority) : Thread(parent,name,priority), UsRangeFinder(parent,name)
    38 {
    39     is_init=false;
    40 
    41     //default values
    42     //range 46*43+43=2021mm max (2m)
    43     //7:gain=118
    44     //range=46;
    45     //gain=7;
    46 
    47     this->i2cport=i2cport;
    48         this->address=address;
    49 
    50     //station sol
    51     gain=new SpinBox(GetGroupBox()->NewRow(),"gain:",0,255,1,8);
    52     range=new SpinBox(GetGroupBox()->LastRowLastCol(),"range:",0,255,1,46);
    53 
    54 
    55     SetRange();
    56     SetMaxGain();
    57 }
    58 
    59 Srf08::~Srf08()
    60 {
    61     SafeStop();
    62     Join();
    63 }
    64 
    65 void Srf08::Run(void)
    66 {
    67     WarnUponSwitches(true);
    68 
    69     //init srf
     32namespace flair {
     33namespace sensor {
     34
     35Srf08::Srf08(const FrameworkManager *parent, string name, I2cPort *i2cport,
     36             uint16_t address, uint8_t priority)
     37    : Thread(parent, name, priority), UsRangeFinder(parent, name) {
     38  is_init = false;
     39
     40  // default values
     41  // range 46*43+43=2021mm max (2m)
     42  // 7:gain=118
     43  // range=46;
     44  // gain=7;
     45
     46  this->i2cport = i2cport;
     47  this->address = address;
     48
     49  // station sol
     50  gain = new SpinBox(GetGroupBox()->NewRow(), "gain:", 0, 255, 1, 8);
     51  range = new SpinBox(GetGroupBox()->LastRowLastCol(), "range:", 0, 255, 1, 46);
     52
     53  SetRange();
     54  SetMaxGain();
     55}
     56
     57Srf08::~Srf08() {
     58  SafeStop();
     59  Join();
     60}
     61
     62void Srf08::Run(void) {
     63  WarnUponSwitches(true);
     64
     65  // init srf
     66  SendEcho();
     67
     68  SetPeriodMS(20);
     69
     70  while (!ToBeStopped()) {
     71    WaitPeriod();
     72
     73    if (range->ValueChanged() == true)
     74      SetRange();
     75    if (gain->ValueChanged() == true)
     76      SetMaxGain();
     77
     78    GetEcho();
    7079    SendEcho();
    71 
    72     SetPeriodMS(20);
    73 
    74     while(!ToBeStopped())
    75     {
    76         WaitPeriod();
    77 
    78         if(range->ValueChanged()==true) SetRange();
    79         if(gain->ValueChanged()==true) SetMaxGain();
    80 
    81         GetEcho();
    82         SendEcho();
     80  }
     81
     82  WarnUponSwitches(false);
     83}
     84
     85void Srf08::SendEcho(void) {
     86  ssize_t written;
     87  uint8_t tx[2];
     88
     89  tx[0] = 0;  // command register
     90  tx[1] = 82; // ranging mode in usec
     91
     92  i2cport->GetMutex();
     93
     94  i2cport->SetSlave(address);
     95  written = i2cport->Write(tx, 2);
     96  echo_time = GetTime();
     97
     98  i2cport->ReleaseMutex();
     99
     100  if (written < 0) {
     101    Thread::Err("erreur rt_dev_write (%s)\n", strerror(-written));
     102  } else if (written != 2) {
     103    Thread::Err("erreur rt_dev_write %i/2\n", written);
     104  }
     105}
     106
     107void Srf08::GetEcho(void) {
     108  float z = 0;
     109  ssize_t written, read;
     110  uint8_t tx, rx[2];
     111  tx = 2;
     112  uint8_t nb_err = 0;
     113
     114  // si l'us est bloqué, on attend 1ms de plus
     115  while (1) {
     116    i2cport->GetMutex();
     117    i2cport->SetSlave(address);
     118    written = i2cport->Write(&tx, 1);
     119
     120    if (written < 0) {
     121      i2cport->ReleaseMutex();
     122      Thread::Err("erreur rt_dev_write (%s)\n", strerror(-written));
     123      nb_err++;
     124      if (nb_err == 20) {
     125        Thread::Err("erreur rt_dev_write (%s), too many errors\n",
     126                    strerror(-written));
     127        return;
     128      }
     129      SleepMS(1);
     130    } else {
     131      read = i2cport->Read(rx, 2);
     132      i2cport->ReleaseMutex();
     133      // rt_printf("%i %i\n",rx[0],rx[1]);
     134      if (read < 0) {
     135        if (read != -ETIMEDOUT)
     136          Thread::Err("erreur rt_dev_read (%s) %i\n", strerror(-written), read);
     137        nb_err++;
     138        if (nb_err == 20) {
     139          Thread::Err("erreur rt_dev_read (%s), too many errors\n",
     140                      strerror(-written));
     141          return;
     142        }
     143        SleepMS(1);
     144      } else if (read != 2) {
     145        Thread::Err("erreur rt_dev_read %i/2\n", read);
     146        return;
     147      } else if (read == 2) {
     148        break;
     149      }
    83150    }
    84 
    85     WarnUponSwitches(false);
    86 }
    87 
    88 void Srf08::SendEcho(void)
    89 {
    90     ssize_t written;
    91     uint8_t tx[2];
    92 
    93     tx[0]=0;//command register
    94     tx[1]=82;//ranging mode in usec
    95 
    96     i2cport->GetMutex();
    97 
    98     i2cport->SetSlave(address);
    99     written = i2cport->Write(tx, 2);
    100     echo_time=GetTime();
    101 
    102     i2cport->ReleaseMutex();
    103 
    104     if(written<0)
    105     {
    106         Thread::Err("erreur rt_dev_write (%s)\n",strerror(-written));
     151  }
     152
     153  // if(z!=-1)
     154  // revoir ce filtrage!!!
     155  {
     156    z = 0.000001 * (float)(rx[0] * 256 + rx[1]) * 344 /
     157        2; // d=v*t; v=344m/s, t=t usec/2 (aller retour)
     158    // if(z>1) rt_printf("%i %i %f\n",rx[0],rx[1],z);
     159    // on ne permet pas 2 mesures consecutives + grandes de 10cm
     160    if (fabs(z - z_1) > 0.5 && is_init == true) {
     161      Printf("z %f (anc %f) %lld\n", z, z_1, echo_time);
     162      Printf("a revoir on suppose le sol plan\n");
     163      z = z_1 + (z_1 - z_2); // on suppose que l'on continue a la meme vitesse
    107164    }
    108     else if (written != 2)
    109     {
    110         Thread::Err("erreur rt_dev_write %i/2\n",written);
    111     }
    112 
    113 }
    114 
    115 void Srf08::GetEcho(void)
    116 {
    117     float z=0;
    118     ssize_t written,read;
    119     uint8_t tx,rx[2];
    120     tx=2;
    121     uint8_t nb_err=0;
    122 
    123     //si l'us est bloqué, on attend 1ms de plus
    124     while(1)
    125     {
    126         i2cport->GetMutex();
    127         i2cport->SetSlave(address);
    128         written = i2cport->Write(&tx, 1);
    129 
    130         if(written<0)
    131         {
    132             i2cport->ReleaseMutex();
    133             Thread::Err("erreur rt_dev_write (%s)\n",strerror(-written));
    134             nb_err++;
    135             if(nb_err==20)
    136             {
    137                 Thread::Err("erreur rt_dev_write (%s), too many errors\n",strerror(-written));
    138                 return;
    139             }
    140             SleepMS(1);
    141         }
    142         else
    143         {
    144             read = i2cport->Read(rx, 2);
    145             i2cport->ReleaseMutex();
    146         //rt_printf("%i %i\n",rx[0],rx[1]);
    147             if(read<0)
    148             {
    149                 if(read!=-ETIMEDOUT) Thread::Err("erreur rt_dev_read (%s) %i\n",strerror(-written),read);
    150                 nb_err++;
    151                 if(nb_err==20)
    152                 {
    153                     Thread::Err("erreur rt_dev_read (%s), too many errors\n",strerror(-written));
    154                     return;
    155                 }
    156                 SleepMS(1);
    157             }
    158             else if (read != 2)
    159             {
    160                 Thread::Err("erreur rt_dev_read %i/2\n",read);
    161                 return;
    162             }
    163             else if(read==2)
    164             {
    165                 break;
    166             }
    167         }
    168     }
    169 
    170     //if(z!=-1)
    171     //revoir ce filtrage!!!
    172     {
    173         z=0.000001*(float)(rx[0]*256+rx[1])*344/2;//d=v*t; v=344m/s, t=t usec/2 (aller retour)
    174 //if(z>1) rt_printf("%i %i %f\n",rx[0],rx[1],z);
    175         //on ne permet pas 2 mesures consecutives + grandes de 10cm
    176         if(fabs(z-z_1)>0.5 && is_init==true)
    177         {
    178             Printf("z %f (anc %f) %lld\n",z,z_1,echo_time);
    179             Printf("a revoir on suppose le sol plan\n");
    180             z=z_1+(z_1-z_2);//on suppose que l'on continue a la meme vitesse
    181         }
    182         output->SetValue(0,0,z);
    183         output->SetDataTime(echo_time+(rx[0]*256+rx[1])*1000);
    184         ProcessUpdate(output);
    185         z_2=z_1;
    186         z_1=z;
    187         is_init=true;
    188     }
    189 }
    190 
    191 
    192 void Srf08::SetRange(void)
    193 {
    194     ssize_t written;
    195     uint8_t tx[2];
    196 
    197     tx[0]=2;//range register
    198     tx[1]=range->Value();//range*43+43=dist max en mm
    199 
    200     i2cport->GetMutex();
    201 
    202     i2cport->SetSlave(address);
    203     written = i2cport->Write(tx, 2);
    204 
    205     i2cport->ReleaseMutex();
    206 
    207     if(written<0)
    208     {
    209         Thread::Err("erreur rt_dev_write (%s)\n",strerror(-written));
    210     }
    211     else if (written != 2)
    212     {
    213         Thread::Err("erreur rt_dev_write %i/2\n",written);
    214     }
    215 
    216 }
    217 
    218 void Srf08::SetMaxGain(void)
    219 {
    220     ssize_t written;
    221     uint8_t tx[2];
    222 
    223     //rt_printf("Srf08::SetMaxGain: %s ->%i\n",IODevice::ObjectName().c_str(),gain->Value());
    224 
    225     tx[0]=1;//max gain register
    226     tx[1]=gain->Value();
    227 
    228     i2cport->GetMutex();
    229 
    230     i2cport->SetSlave(address);
    231     written = i2cport->Write(tx, 2);
    232 
    233     i2cport->ReleaseMutex();
    234 
    235     if(written<0)
    236     {
    237         Thread::Err("erreur write (%s)\n",strerror(-written));
    238     }
    239     else if (written != 2)
    240     {
    241         Thread::Err("erreur write %i/2\n",written);
    242     }
     165    output->SetValue(0, 0, z);
     166    output->SetDataTime(echo_time + (rx[0] * 256 + rx[1]) * 1000);
     167    ProcessUpdate(output);
     168    z_2 = z_1;
     169    z_1 = z;
     170    is_init = true;
     171  }
     172}
     173
     174void Srf08::SetRange(void) {
     175  ssize_t written;
     176  uint8_t tx[2];
     177
     178  tx[0] = 2;              // range register
     179  tx[1] = range->Value(); // range*43+43=dist max en mm
     180
     181  i2cport->GetMutex();
     182
     183  i2cport->SetSlave(address);
     184  written = i2cport->Write(tx, 2);
     185
     186  i2cport->ReleaseMutex();
     187
     188  if (written < 0) {
     189    Thread::Err("erreur rt_dev_write (%s)\n", strerror(-written));
     190  } else if (written != 2) {
     191    Thread::Err("erreur rt_dev_write %i/2\n", written);
     192  }
     193}
     194
     195void Srf08::SetMaxGain(void) {
     196  ssize_t written;
     197  uint8_t tx[2];
     198
     199  // rt_printf("Srf08::SetMaxGain: %s
     200  // ->%i\n",IODevice::ObjectName().c_str(),gain->Value());
     201
     202  tx[0] = 1; // max gain register
     203  tx[1] = gain->Value();
     204
     205  i2cport->GetMutex();
     206
     207  i2cport->SetSlave(address);
     208  written = i2cport->Write(tx, 2);
     209
     210  i2cport->ReleaseMutex();
     211
     212  if (written < 0) {
     213    Thread::Err("erreur write (%s)\n", strerror(-written));
     214  } else if (written != 2) {
     215    Thread::Err("erreur write %i/2\n", written);
     216  }
    243217}
    244218
Note: See TracChangeset for help on using the changeset viewer.