[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>
|
---|
| 23 | #include <cvmatrix.h>
|
---|
| 24 | #include <string.h>
|
---|
| 25 | #include <errno.h>
|
---|
| 26 | #include <math.h>
|
---|
| 27 |
|
---|
| 28 | using std::string;
|
---|
| 29 | using namespace flair::core;
|
---|
| 30 | using namespace flair::gui;
|
---|
| 31 |
|
---|
[15] | 32 | namespace flair {
|
---|
| 33 | namespace sensor {
|
---|
[3] | 34 |
|
---|
[137] | 35 | Srf08::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] | 59 | Srf08::~Srf08() {
|
---|
| 60 | SafeStop();
|
---|
| 61 | Join();
|
---|
[3] | 62 | }
|
---|
| 63 |
|
---|
[15] | 64 | void 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] | 87 | void 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] | 109 | void 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] | 176 | void 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] | 197 | void 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
|
---|