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

Last change on this file was 268, checked in by Sanahuja Guillaume, 3 years ago

add defines for architectures to speed up compile time

File size: 5.0 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
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#ifdef ARMV7A
18
19#include "Srf08.h"
20#include <I2cPort.h>
21#include <FrameworkManager.h>
22#include <GroupBox.h>
23#include <SpinBox.h>
24#include <Matrix.h>
25#include <string.h>
26#include <errno.h>
27#include <math.h>
28
29using std::string;
30using namespace flair::core;
31using namespace flair::gui;
32
33namespace flair {
34namespace sensor {
35
36Srf08::Srf08(string name, I2cPort *i2cport,
37             uint16_t address, uint8_t priority)
38    : Thread(getFrameworkManager(), name, priority), UsRangeFinder(name) {
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  SetRange();
55  SetMaxGain();
56 
57  SetIsReady(true);
58}
59
60Srf08::~Srf08() {
61  SafeStop();
62  Join();
63}
64
65void Srf08::Run(void) {
66  WarnUponSwitches(true);
67
68  // init srf
69  SendEcho();
70
71  SetPeriodMS(20);
72
73  while (!ToBeStopped()) {
74    WaitPeriod();
75
76    if (range->ValueChanged() == true)
77      SetRange();
78    if (gain->ValueChanged() == true)
79      SetMaxGain();
80
81    GetEcho();
82    SendEcho();
83  }
84
85  WarnUponSwitches(false);
86}
87
88void Srf08::SendEcho(void) {
89  ssize_t written;
90  uint8_t tx[2];
91
92  tx[0] = 0;  // command register
93  tx[1] = 82; // ranging mode in usec
94
95  i2cport->GetMutex();
96
97  i2cport->SetSlave(address);
98  written = i2cport->Write(tx, 2);
99  echo_time = GetTime();
100
101  i2cport->ReleaseMutex();
102
103  if (written < 0) {
104    Thread::Err("error write i2c (%s)\n", strerror(-written));
105  } else if (written != 2) {
106    Thread::Err("error write i2c %i/2\n", written);
107  }
108}
109
110void Srf08::GetEcho(void) {
111  float z = 0;
112  ssize_t written, read;
113  uint8_t tx, rx[2];
114  tx = 2;
115  uint8_t nb_err = 0;
116
117  // si l'us est bloqué, on attend 1ms de plus
118  while (1) {
119    i2cport->GetMutex();
120    i2cport->SetSlave(address);
121    written = i2cport->Write(&tx, 1);
122
123    if (written < 0) {
124      i2cport->ReleaseMutex();
125      Thread::Err("error write i2c (%s)\n", strerror(-written));
126      nb_err++;
127      if (nb_err == 20) {
128        Thread::Err("error write i2c (%s), too many errors\n",
129                    strerror(-written));
130        return;
131      }
132      SleepMS(1);
133    } else {
134      read = i2cport->Read(rx, 2);
135      i2cport->ReleaseMutex();
136      // rt_printf("%i %i\n",rx[0],rx[1]);
137      if (read < 0) {
138        if (read != -ETIMEDOUT)
139          Thread::Err("error read i2c (%s) %i\n", strerror(-read), read);
140        nb_err++;
141        if (nb_err == 20) {
142          Thread::Err("error read i2c (%s), too many errors\n",
143                      strerror(-written));
144          return;
145        }
146        SleepMS(1);
147      } else if (read != 2) {
148        Thread::Err("error read i2c %i/2\n", read);
149        return;
150      } else if (read == 2) {
151        break;
152      }
153    }
154  }
155
156  // if(z!=-1)
157  // revoir ce filtrage!!!
158  {
159    z = 0.000001 * (float)(rx[0] * 256 + rx[1]) * 344 /
160        2; // d=v*t; v=344m/s, t=t usec/2 (aller retour)
161    // if(z>1) rt_printf("%i %i %f\n",rx[0],rx[1],z);
162    // on ne permet pas 2 mesures consecutives + grandes de 10cm
163    if (fabs(z - z_1) > 0.5 && is_init == true) {
164      Printf("z %f (anc %f) %lld\n", z, z_1, echo_time);
165      Printf("a revoir on suppose le sol plan\n");
166      z = z_1 + (z_1 - z_2); // on suppose que l'on continue a la meme vitesse
167    }
168    output->SetValue(0, 0, z);
169    output->SetDataTime(echo_time + (rx[0] * 256 + rx[1]) * 1000);
170    ProcessUpdate(output);
171    z_2 = z_1;
172    z_1 = z;
173    is_init = true;
174  }
175}
176
177void Srf08::SetRange(void) {
178  ssize_t written;
179  uint8_t tx[2];
180
181  tx[0] = 2;              // range register
182  tx[1] = range->Value(); // range*43+43=dist max en mm
183
184  i2cport->GetMutex();
185
186  i2cport->SetSlave(address);
187  written = i2cport->Write(tx, 2);
188
189  i2cport->ReleaseMutex();
190
191  if (written < 0) {
192    Thread::Err("error write i2c (%s)\n", strerror(-written));
193  } else if (written != 2) {
194    Thread::Err("error write i2c %i/2\n", written);
195  }
196}
197
198void Srf08::SetMaxGain(void) {
199  ssize_t written;
200  uint8_t tx[2];
201
202  // rt_printf("Srf08::SetMaxGain: %s
203  // ->%i\n",IODevice::ObjectName().c_str(),gain->Value());
204
205  tx[0] = 1; // max gain register
206  tx[1] = gain->Value();
207
208  i2cport->GetMutex();
209
210  i2cport->SetSlave(address);
211  written = i2cport->Write(tx, 2);
212
213  i2cport->ReleaseMutex();
214
215  if (written < 0) {
216    Thread::Err("error write i2c (%s)\n", strerror(-written));
217  } else if (written != 2) {
218    Thread::Err("error write i2c %i/2\n", written);
219  }
220}
221
222} // end namespace sensor
223} // end namespace flair
224
225#endif
Note: See TracBrowser for help on using the repository browser.