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

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

matrix

File size: 4.9 KB
RevLine 
[3]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[3]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>
[214]23#include <Matrix.h>
[3]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
[15]32namespace flair {
33namespace sensor {
[3]34
[137]35Srf08::Srf08(string name, I2cPort *i2cport,
[15]36 uint16_t address, uint8_t priority)
[137]37 : Thread(getFrameworkManager(), name, priority), UsRangeFinder(name) {
[15]38 is_init = false;
[3]39
[15]40 // default values
41 // range 46*43+43=2021mm max (2m)
42 // 7:gain=118
43 // range=46;
44 // gain=7;
[3]45
[15]46 this->i2cport = i2cport;
47 this->address = address;
[3]48
[15]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);
[3]52
[15]53 SetRange();
54 SetMaxGain();
[157]55
56 SetIsReady(true);
[3]57}
58
[15]59Srf08::~Srf08() {
60 SafeStop();
61 Join();
[3]62}
63
[15]64void Srf08::Run(void) {
65 WarnUponSwitches(true);
[3]66
[15]67 // init srf
68 SendEcho();
[3]69
[15]70 SetPeriodMS(20);
[3]71
[15]72 while (!ToBeStopped()) {
73 WaitPeriod();
[3]74
[15]75 if (range->ValueChanged() == true)
76 SetRange();
77 if (gain->ValueChanged() == true)
78 SetMaxGain();
[3]79
[15]80 GetEcho();
81 SendEcho();
82 }
[3]83
[15]84 WarnUponSwitches(false);
[3]85}
86
[15]87void Srf08::SendEcho(void) {
88 ssize_t written;
89 uint8_t tx[2];
[3]90
[15]91 tx[0] = 0; // command register
92 tx[1] = 82; // ranging mode in usec
[3]93
[15]94 i2cport->GetMutex();
[3]95
[15]96 i2cport->SetSlave(address);
97 written = i2cport->Write(tx, 2);
98 echo_time = GetTime();
[3]99
[15]100 i2cport->ReleaseMutex();
[3]101
[15]102 if (written < 0) {
[51]103 Thread::Err("error write i2c (%s)\n", strerror(-written));
[15]104 } else if (written != 2) {
[51]105 Thread::Err("error write i2c %i/2\n", written);
[15]106 }
[3]107}
108
[15]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;
[3]115
[15]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);
[3]121
[15]122 if (written < 0) {
123 i2cport->ReleaseMutex();
[51]124 Thread::Err("error write i2c (%s)\n", strerror(-written));
[15]125 nb_err++;
126 if (nb_err == 20) {
[51]127 Thread::Err("error write i2c (%s), too many errors\n",
[15]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)
[51]138 Thread::Err("error read i2c (%s) %i\n", strerror(-read), read);
[15]139 nb_err++;
140 if (nb_err == 20) {
[51]141 Thread::Err("error read i2c (%s), too many errors\n",
[15]142 strerror(-written));
143 return;
[3]144 }
[15]145 SleepMS(1);
146 } else if (read != 2) {
[51]147 Thread::Err("error read i2c %i/2\n", read);
[15]148 return;
149 } else if (read == 2) {
150 break;
151 }
[3]152 }
[15]153 }
[3]154
[15]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
[3]166 }
[15]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 }
[3]174}
175
[15]176void Srf08::SetRange(void) {
177 ssize_t written;
178 uint8_t tx[2];
[3]179
[15]180 tx[0] = 2; // range register
181 tx[1] = range->Value(); // range*43+43=dist max en mm
[3]182
[15]183 i2cport->GetMutex();
[3]184
[15]185 i2cport->SetSlave(address);
186 written = i2cport->Write(tx, 2);
[3]187
[15]188 i2cport->ReleaseMutex();
[3]189
[15]190 if (written < 0) {
[51]191 Thread::Err("error write i2c (%s)\n", strerror(-written));
[15]192 } else if (written != 2) {
[51]193 Thread::Err("error write i2c %i/2\n", written);
[15]194 }
[3]195}
196
[15]197void Srf08::SetMaxGain(void) {
198 ssize_t written;
199 uint8_t tx[2];
[3]200
[15]201 // rt_printf("Srf08::SetMaxGain: %s
202 // ->%i\n",IODevice::ObjectName().c_str(),gain->Value());
[3]203
[15]204 tx[0] = 1; // max gain register
205 tx[1] = gain->Value();
[3]206
[15]207 i2cport->GetMutex();
[3]208
[15]209 i2cport->SetSlave(address);
210 written = i2cport->Write(tx, 2);
[3]211
[15]212 i2cport->ReleaseMutex();
[3]213
[15]214 if (written < 0) {
[51]215 Thread::Err("error write i2c (%s)\n", strerror(-written));
[15]216 } else if (written != 2) {
[51]217 Thread::Err("error write i2c %i/2\n", written);
[15]218 }
[3]219}
220
221} // end namespace sensor
222} // end namespace flair
Note: See TracBrowser for help on using the repository browser.