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

Last change on this file since 137 was 137, checked in by Sanahuja Guillaume, 6 years ago

singleton manager

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
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();
79 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("error write i2c (%s)\n", strerror(-written));
102 } else if (written != 2) {
103 Thread::Err("error write i2c %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("error write i2c (%s)\n", strerror(-written));
123 nb_err++;
124 if (nb_err == 20) {
125 Thread::Err("error write i2c (%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("error read i2c (%s) %i\n", strerror(-read), read);
137 nb_err++;
138 if (nb_err == 20) {
139 Thread::Err("error read i2c (%s), too many errors\n",
140 strerror(-written));
141 return;
142 }
143 SleepMS(1);
144 } else if (read != 2) {
145 Thread::Err("error read i2c %i/2\n", read);
146 return;
147 } else if (read == 2) {
148 break;
149 }
150 }
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
164 }
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("error write i2c (%s)\n", strerror(-written));
190 } else if (written != 2) {
191 Thread::Err("error write i2c %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("error write i2c (%s)\n", strerror(-written));
214 } else if (written != 2) {
215 Thread::Err("error write i2c %i/2\n", written);
216 }
217}
218
219} // end namespace sensor
220} // end namespace flair
Note: See TracBrowser for help on using the repository browser.