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