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

Last change on this file since 268 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.