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

Last change on this file since 157 was 157, checked in by Sanahuja Guillaume, 4 years ago

iadded isready to iodevice:
avoid problem of imu not ready in ardrone2

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