[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 | /*********************************************************************/
|
---|
[268] | 17 | #ifdef ARMV7A
|
---|
[3] | 18 |
|
---|
| 19 | #include "Srf08.h"
|
---|
| 20 | #include <I2cPort.h>
|
---|
| 21 | #include <FrameworkManager.h>
|
---|
| 22 | #include <GroupBox.h>
|
---|
| 23 | #include <SpinBox.h>
|
---|
[214] | 24 | #include <Matrix.h>
|
---|
[3] | 25 | #include <string.h>
|
---|
| 26 | #include <errno.h>
|
---|
| 27 | #include <math.h>
|
---|
| 28 |
|
---|
| 29 | using std::string;
|
---|
| 30 | using namespace flair::core;
|
---|
| 31 | using namespace flair::gui;
|
---|
| 32 |
|
---|
[15] | 33 | namespace flair {
|
---|
| 34 | namespace sensor {
|
---|
[3] | 35 |
|
---|
[137] | 36 | Srf08::Srf08(string name, I2cPort *i2cport,
|
---|
[15] | 37 | uint16_t address, uint8_t priority)
|
---|
[137] | 38 | : Thread(getFrameworkManager(), name, priority), UsRangeFinder(name) {
|
---|
[15] | 39 | is_init = false;
|
---|
[3] | 40 |
|
---|
[15] | 41 | // default values
|
---|
| 42 | // range 46*43+43=2021mm max (2m)
|
---|
| 43 | // 7:gain=118
|
---|
| 44 | // range=46;
|
---|
| 45 | // gain=7;
|
---|
[3] | 46 |
|
---|
[15] | 47 | this->i2cport = i2cport;
|
---|
| 48 | this->address = address;
|
---|
[3] | 49 |
|
---|
[15] | 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);
|
---|
[3] | 53 |
|
---|
[15] | 54 | SetRange();
|
---|
| 55 | SetMaxGain();
|
---|
[157] | 56 |
|
---|
| 57 | SetIsReady(true);
|
---|
[3] | 58 | }
|
---|
| 59 |
|
---|
[15] | 60 | Srf08::~Srf08() {
|
---|
| 61 | SafeStop();
|
---|
| 62 | Join();
|
---|
[3] | 63 | }
|
---|
| 64 |
|
---|
[15] | 65 | void Srf08::Run(void) {
|
---|
| 66 | WarnUponSwitches(true);
|
---|
[3] | 67 |
|
---|
[15] | 68 | // init srf
|
---|
| 69 | SendEcho();
|
---|
[3] | 70 |
|
---|
[15] | 71 | SetPeriodMS(20);
|
---|
[3] | 72 |
|
---|
[15] | 73 | while (!ToBeStopped()) {
|
---|
| 74 | WaitPeriod();
|
---|
[3] | 75 |
|
---|
[15] | 76 | if (range->ValueChanged() == true)
|
---|
| 77 | SetRange();
|
---|
| 78 | if (gain->ValueChanged() == true)
|
---|
| 79 | SetMaxGain();
|
---|
[3] | 80 |
|
---|
[15] | 81 | GetEcho();
|
---|
| 82 | SendEcho();
|
---|
| 83 | }
|
---|
[3] | 84 |
|
---|
[15] | 85 | WarnUponSwitches(false);
|
---|
[3] | 86 | }
|
---|
| 87 |
|
---|
[15] | 88 | void Srf08::SendEcho(void) {
|
---|
| 89 | ssize_t written;
|
---|
| 90 | uint8_t tx[2];
|
---|
[3] | 91 |
|
---|
[15] | 92 | tx[0] = 0; // command register
|
---|
| 93 | tx[1] = 82; // ranging mode in usec
|
---|
[3] | 94 |
|
---|
[15] | 95 | i2cport->GetMutex();
|
---|
[3] | 96 |
|
---|
[15] | 97 | i2cport->SetSlave(address);
|
---|
| 98 | written = i2cport->Write(tx, 2);
|
---|
| 99 | echo_time = GetTime();
|
---|
[3] | 100 |
|
---|
[15] | 101 | i2cport->ReleaseMutex();
|
---|
[3] | 102 |
|
---|
[15] | 103 | if (written < 0) {
|
---|
[51] | 104 | Thread::Err("error write i2c (%s)\n", strerror(-written));
|
---|
[15] | 105 | } else if (written != 2) {
|
---|
[51] | 106 | Thread::Err("error write i2c %i/2\n", written);
|
---|
[15] | 107 | }
|
---|
[3] | 108 | }
|
---|
| 109 |
|
---|
[15] | 110 | void 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;
|
---|
[3] | 116 |
|
---|
[15] | 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);
|
---|
[3] | 122 |
|
---|
[15] | 123 | if (written < 0) {
|
---|
| 124 | i2cport->ReleaseMutex();
|
---|
[51] | 125 | Thread::Err("error write i2c (%s)\n", strerror(-written));
|
---|
[15] | 126 | nb_err++;
|
---|
| 127 | if (nb_err == 20) {
|
---|
[51] | 128 | Thread::Err("error write i2c (%s), too many errors\n",
|
---|
[15] | 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)
|
---|
[51] | 139 | Thread::Err("error read i2c (%s) %i\n", strerror(-read), read);
|
---|
[15] | 140 | nb_err++;
|
---|
| 141 | if (nb_err == 20) {
|
---|
[51] | 142 | Thread::Err("error read i2c (%s), too many errors\n",
|
---|
[15] | 143 | strerror(-written));
|
---|
| 144 | return;
|
---|
[3] | 145 | }
|
---|
[15] | 146 | SleepMS(1);
|
---|
| 147 | } else if (read != 2) {
|
---|
[51] | 148 | Thread::Err("error read i2c %i/2\n", read);
|
---|
[15] | 149 | return;
|
---|
| 150 | } else if (read == 2) {
|
---|
| 151 | break;
|
---|
| 152 | }
|
---|
[3] | 153 | }
|
---|
[15] | 154 | }
|
---|
[3] | 155 |
|
---|
[15] | 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
|
---|
[3] | 167 | }
|
---|
[15] | 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 | }
|
---|
[3] | 175 | }
|
---|
| 176 |
|
---|
[15] | 177 | void Srf08::SetRange(void) {
|
---|
| 178 | ssize_t written;
|
---|
| 179 | uint8_t tx[2];
|
---|
[3] | 180 |
|
---|
[15] | 181 | tx[0] = 2; // range register
|
---|
| 182 | tx[1] = range->Value(); // range*43+43=dist max en mm
|
---|
[3] | 183 |
|
---|
[15] | 184 | i2cport->GetMutex();
|
---|
[3] | 185 |
|
---|
[15] | 186 | i2cport->SetSlave(address);
|
---|
| 187 | written = i2cport->Write(tx, 2);
|
---|
[3] | 188 |
|
---|
[15] | 189 | i2cport->ReleaseMutex();
|
---|
[3] | 190 |
|
---|
[15] | 191 | if (written < 0) {
|
---|
[51] | 192 | Thread::Err("error write i2c (%s)\n", strerror(-written));
|
---|
[15] | 193 | } else if (written != 2) {
|
---|
[51] | 194 | Thread::Err("error write i2c %i/2\n", written);
|
---|
[15] | 195 | }
|
---|
[3] | 196 | }
|
---|
| 197 |
|
---|
[15] | 198 | void Srf08::SetMaxGain(void) {
|
---|
| 199 | ssize_t written;
|
---|
| 200 | uint8_t tx[2];
|
---|
[3] | 201 |
|
---|
[15] | 202 | // rt_printf("Srf08::SetMaxGain: %s
|
---|
| 203 | // ->%i\n",IODevice::ObjectName().c_str(),gain->Value());
|
---|
[3] | 204 |
|
---|
[15] | 205 | tx[0] = 1; // max gain register
|
---|
| 206 | tx[1] = gain->Value();
|
---|
[3] | 207 |
|
---|
[15] | 208 | i2cport->GetMutex();
|
---|
[3] | 209 |
|
---|
[15] | 210 | i2cport->SetSlave(address);
|
---|
| 211 | written = i2cport->Write(tx, 2);
|
---|
[3] | 212 |
|
---|
[15] | 213 | i2cport->ReleaseMutex();
|
---|
[3] | 214 |
|
---|
[15] | 215 | if (written < 0) {
|
---|
[51] | 216 | Thread::Err("error write i2c (%s)\n", strerror(-written));
|
---|
[15] | 217 | } else if (written != 2) {
|
---|
[51] | 218 | Thread::Err("error write i2c %i/2\n", written);
|
---|
[15] | 219 | }
|
---|
[3] | 220 | }
|
---|
| 221 |
|
---|
| 222 | } // end namespace sensor
|
---|
| 223 | } // end namespace flair
|
---|
[268] | 224 |
|
---|
| 225 | #endif
|
---|