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

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

sensoractuator

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