source: flair-src/trunk/lib/FlairSensorActuator/src/Srf08.cpp @ 98

Last change on this file since 98 was 51, checked in by Sanahuja Guillaume, 5 years ago

gps

File size: 4.9 KB
RevLine 
[3]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[3]4// %flair:license}
5//  created:    2011/05/01
6//  filename:   Srf08.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    Class for ultra sonic SRF08
14//
15//
16/*********************************************************************/
17
18#include "Srf08.h"
19#include <I2cPort.h>
20#include <FrameworkManager.h>
21#include <GroupBox.h>
22#include <SpinBox.h>
23#include <cvmatrix.h>
24#include <string.h>
25#include <errno.h>
26#include <math.h>
27
28using std::string;
29using namespace flair::core;
30using namespace flair::gui;
31
[15]32namespace flair {
33namespace sensor {
[3]34
[15]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;
[3]39
[15]40  // default values
41  // range 46*43+43=2021mm max (2m)
42  // 7:gain=118
43  // range=46;
44  // gain=7;
[3]45
[15]46  this->i2cport = i2cport;
47  this->address = address;
[3]48
[15]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);
[3]52
[15]53  SetRange();
54  SetMaxGain();
[3]55}
56
[15]57Srf08::~Srf08() {
58  SafeStop();
59  Join();
[3]60}
61
[15]62void Srf08::Run(void) {
63  WarnUponSwitches(true);
[3]64
[15]65  // init srf
66  SendEcho();
[3]67
[15]68  SetPeriodMS(20);
[3]69
[15]70  while (!ToBeStopped()) {
71    WaitPeriod();
[3]72
[15]73    if (range->ValueChanged() == true)
74      SetRange();
75    if (gain->ValueChanged() == true)
76      SetMaxGain();
[3]77
[15]78    GetEcho();
79    SendEcho();
80  }
[3]81
[15]82  WarnUponSwitches(false);
[3]83}
84
[15]85void Srf08::SendEcho(void) {
86  ssize_t written;
87  uint8_t tx[2];
[3]88
[15]89  tx[0] = 0;  // command register
90  tx[1] = 82; // ranging mode in usec
[3]91
[15]92  i2cport->GetMutex();
[3]93
[15]94  i2cport->SetSlave(address);
95  written = i2cport->Write(tx, 2);
96  echo_time = GetTime();
[3]97
[15]98  i2cport->ReleaseMutex();
[3]99
[15]100  if (written < 0) {
[51]101    Thread::Err("error write i2c (%s)\n", strerror(-written));
[15]102  } else if (written != 2) {
[51]103    Thread::Err("error write i2c %i/2\n", written);
[15]104  }
[3]105}
106
[15]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;
[3]113
[15]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);
[3]119
[15]120    if (written < 0) {
121      i2cport->ReleaseMutex();
[51]122      Thread::Err("error write i2c (%s)\n", strerror(-written));
[15]123      nb_err++;
124      if (nb_err == 20) {
[51]125        Thread::Err("error write i2c (%s), too many errors\n",
[15]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)
[51]136          Thread::Err("error read i2c (%s) %i\n", strerror(-read), read);
[15]137        nb_err++;
138        if (nb_err == 20) {
[51]139          Thread::Err("error read i2c (%s), too many errors\n",
[15]140                      strerror(-written));
141          return;
[3]142        }
[15]143        SleepMS(1);
144      } else if (read != 2) {
[51]145        Thread::Err("error read i2c %i/2\n", read);
[15]146        return;
147      } else if (read == 2) {
148        break;
149      }
[3]150    }
[15]151  }
[3]152
[15]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
[3]164    }
[15]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  }
[3]172}
173
[15]174void Srf08::SetRange(void) {
175  ssize_t written;
176  uint8_t tx[2];
[3]177
[15]178  tx[0] = 2;              // range register
179  tx[1] = range->Value(); // range*43+43=dist max en mm
[3]180
[15]181  i2cport->GetMutex();
[3]182
[15]183  i2cport->SetSlave(address);
184  written = i2cport->Write(tx, 2);
[3]185
[15]186  i2cport->ReleaseMutex();
[3]187
[15]188  if (written < 0) {
[51]189    Thread::Err("error write i2c (%s)\n", strerror(-written));
[15]190  } else if (written != 2) {
[51]191    Thread::Err("error write i2c %i/2\n", written);
[15]192  }
[3]193}
194
[15]195void Srf08::SetMaxGain(void) {
196  ssize_t written;
197  uint8_t tx[2];
[3]198
[15]199  // rt_printf("Srf08::SetMaxGain: %s
200  // ->%i\n",IODevice::ObjectName().c_str(),gain->Value());
[3]201
[15]202  tx[0] = 1; // max gain register
203  tx[1] = gain->Value();
[3]204
[15]205  i2cport->GetMutex();
[3]206
[15]207  i2cport->SetSlave(address);
208  written = i2cport->Write(tx, 2);
[3]209
[15]210  i2cport->ReleaseMutex();
[3]211
[15]212  if (written < 0) {
[51]213    Thread::Err("error write i2c (%s)\n", strerror(-written));
[15]214  } else if (written != 2) {
[51]215    Thread::Err("error write i2c %i/2\n", written);
[15]216  }
[3]217}
218
219} // end namespace sensor
220} // end namespace flair
Note: See TracBrowser for help on using the repository browser.