liblms1xx: LMS1xx.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * LMS1xx.cpp
00003  *
00004  *  Created on: 09-08-2010
00005  *  Author: Konrad Banachowicz
00006  ***************************************************************************
00007  *   This library is free software; you can redistribute it and/or         *
00008  *   modify it under the terms of the GNU Lesser General Public            *
00009  *   License as published by the Free Software Foundation; either          *
00010  *   version 2.1 of the License, or (at your option) any later version.    *
00011  *                                                                         *
00012  *   This library is distributed in the hope that it will be useful,       *
00013  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00014  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00015  *   Lesser General Public License for more details.                       *
00016  *                                                                         *
00017  *   You should have received a copy of the GNU Lesser General Public      *
00018  *   License along with this library; if not, write to the Free Software   *
00019  *   Foundation, Inc., 59 Temple Place,                                    *
00020  *   Suite 330, Boston, MA  02111-1307  USA                                *
00021  *                                                                         *
00022  ***************************************************************************/
00023 
00024 #include <sys/types.h>
00025 #include <sys/socket.h>
00026 #include <netinet/in.h>
00027 #include <arpa/inet.h>
00028 #include <cstdio>
00029 #include <cstdlib>
00030 #include <cstring>
00031 #include <unistd.h>
00032 
00033 #include "LMS1xx.h"
00034 
00035 LMS1xx::LMS1xx() :
00036         connected(false) {
00037         debug = false;
00038 }
00039 
00040 LMS1xx::~LMS1xx() {
00041 
00042 }
00043 
00044 void LMS1xx::connect(std::string host, int port) {
00045         if (!connected) {
00046                 sockDesc = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
00047                 if (sockDesc) {
00048                         struct sockaddr_in stSockAddr;
00049                         int Res;
00050                         stSockAddr.sin_family = PF_INET;
00051                         stSockAddr.sin_port = htons(port);
00052                         Res = inet_pton(AF_INET, host.c_str(), &stSockAddr.sin_addr);
00053 
00054                         int ret = ::connect(sockDesc, (struct sockaddr *) &stSockAddr,
00055                                         sizeof stSockAddr);
00056                         if (ret == 0) {
00057                                 connected = true;
00058                         }
00059                 }
00060         }
00061 }
00062 
00063 void LMS1xx::disconnect() {
00064         if (connected) {
00065                 close(sockDesc);
00066                 connected = false;
00067         }
00068 }
00069 
00070 bool LMS1xx::isConnected() {
00071         return connected;
00072 }
00073 
00074 void LMS1xx::startMeas() {
00075         char buf[100];
00076         sprintf(buf, "%c%s%c", 0x02, "sMN LMCstartmeas", 0x03);
00077 
00078         write(sockDesc, buf, strlen(buf));
00079 
00080         int len = read(sockDesc, buf, 100);
00081         //      if (buf[0] != 0x02)
00082         //              std::cout << "invalid packet recieved" << std::endl;
00083         //      if (debug) {
00084         //              buf[len] = 0;
00085         //              std::cout << buf << std::endl;
00086         //      }
00087 }
00088 
00089 void LMS1xx::stopMeas() {
00090         char buf[100];
00091         sprintf(buf, "%c%s%c", 0x02, "sMN LMCstopmeas", 0x03);
00092 
00093         write(sockDesc, buf, strlen(buf));
00094 
00095         int len = read(sockDesc, buf, 100);
00096         //      if (buf[0] != 0x02)
00097         //              std::cout << "invalid packet recieved" << std::endl;
00098         //      if (debug) {
00099         //              buf[len] = 0;
00100         //              std::cout << buf << std::endl;
00101         //      }
00102 }
00103 
00104 status_t LMS1xx::queryStatus() {
00105         char buf[100];
00106         sprintf(buf, "%c%s%c", 0x02, "sRN STlms", 0x03);
00107 
00108         write(sockDesc, buf, strlen(buf));
00109 
00110         int len = read(sockDesc, buf, 100);
00111         //      if (buf[0] != 0x02)
00112         //              std::cout << "invalid packet recieved" << std::endl;
00113         //      if (debug) {
00114         //              buf[len] = 0;
00115         //              std::cout << buf << std::endl;
00116         //      }
00117         int ret;
00118         sscanf((buf + 10), "%d", &ret);
00119 
00120         return (status_t) ret;
00121 }
00122 
00123 void LMS1xx::login() {
00124         char buf[100];
00125         sprintf(buf, "%c%s%c", 0x02, "sMN SetAccessMode 03 F4724744", 0x03);
00126 
00127         write(sockDesc, buf, strlen(buf));
00128 
00129         int len = read(sockDesc, buf, 100);
00130         //      if (buf[0] != 0x02)
00131         //              std::cout << "invalid packet recieved" << std::endl;
00132         //      if (debug) {
00133         //              buf[len] = 0;
00134         //              std::cout << buf << std::endl;
00135         //      }
00136 }
00137 
00138 scanCfg LMS1xx::getScanCfg() const {
00139         scanCfg cfg;
00140         char buf[100];
00141         sprintf(buf, "%c%s%c", 0x02, "sRN LMPscancfg", 0x03);
00142 
00143         write(sockDesc, buf, strlen(buf));
00144 
00145         int len = read(sockDesc, buf, 100);
00146         //      if (buf[0] != 0x02)
00147         //              std::cout << "invalid packet recieved" << std::endl;
00148         //      if (debug) {
00149         //              buf[len] = 0;
00150         //              std::cout << buf << std::endl;
00151         //      }
00152 
00153         sscanf(buf + 1, "%*s %*s %X %*d %X %X %X", &cfg.scaningFrequency,
00154                         &cfg.angleResolution, &cfg.startAngle, &cfg.stopAngle);
00155         return cfg;
00156 }
00157 
00158 void LMS1xx::setScanCfg(const scanCfg &cfg) {
00159         char buf[100];
00160         sprintf(buf, "%c%s %X +1 %X %X %X%c", 0x02, "sMN mLMPsetscancfg",
00161                         cfg.scaningFrequency, cfg.angleResolution, cfg.startAngle,
00162                         cfg.stopAngle, 0x03);
00163 
00164         write(sockDesc, buf, strlen(buf));
00165 
00166         int len = read(sockDesc, buf, 100);
00167 
00168         buf[len - 1] = 0;
00169 }
00170 
00171 void LMS1xx::setScanDataCfg(const scanDataCfg &cfg) {
00172         char buf[100];
00173         sprintf(buf, "%c%s %02X 00 %d %d 0 %02X 00 %d %d 0 %d +%d%c", 0x02,
00174                         "sWN LMDscandatacfg", cfg.outputChannel, cfg.remission ? 1 : 0,
00175                         cfg.resolution, cfg.encoder, cfg.position ? 1 : 0,
00176                         cfg.deviceName ? 1 : 0, cfg.timestamp ? 1 : 0, cfg.outputInterval, 0x03);
00177         if(debug)
00178                 printf("%s\n", buf);
00179         write(sockDesc, buf, strlen(buf));
00180 
00181         int len = read(sockDesc, buf, 100);
00182         buf[len - 1] = 0;
00183 }
00184 
00185 void LMS1xx::scanContinous(int start) {
00186         char buf[100];
00187         sprintf(buf, "%c%s %d%c", 0x02, "sEN LMDscandata", start, 0x03);
00188 
00189         write(sockDesc, buf, strlen(buf));
00190 
00191         int len = read(sockDesc, buf, 100);
00192 
00193         if (buf[0] != 0x02)
00194                 printf("invalid packet recieved\n");
00195 
00196         if (debug) {
00197                 buf[len] = 0;
00198                 printf("%s\n", buf);
00199         }
00200 
00201         if (start = 0) {
00202                 for (int i = 0; i < 10; i++)
00203                         read(sockDesc, buf, 100);
00204         }
00205 }
00206 
00207 void LMS1xx::getData(scanData& data) {
00208         char buf[20000];
00209         fd_set rfds;
00210         struct timeval tv;
00211         int retval, len;
00212         len = 0;
00213 
00214         do {
00215                 FD_ZERO(&rfds);
00216                 FD_SET(sockDesc, &rfds);
00217 
00218                 tv.tv_sec = 0;
00219                 tv.tv_usec = 50000;
00220                 retval = select(sockDesc + 1, &rfds, NULL, NULL, &tv);
00221                 if (retval) {
00222                         len += read(sockDesc, buf + len, 20000 - len);
00223                 }
00224         } while ((buf[0] != 0x02) || (buf[len - 1] != 0x03));
00225 
00226         //      if (debug)
00227         //              std::cout << "scan data recieved" << std::endl;
00228         buf[len - 1] = 0;
00229         char* tok = strtok(buf, " "); //Type of command
00230         tok = strtok(NULL, " "); //Command
00231         tok = strtok(NULL, " "); //VersionNumber
00232         tok = strtok(NULL, " "); //DeviceNumber
00233         tok = strtok(NULL, " "); //Serial number
00234         tok = strtok(NULL, " "); //DeviceStatus
00235         tok = strtok(NULL, " "); //MessageCounter
00236         tok = strtok(NULL, " "); //ScanCounter
00237         tok = strtok(NULL, " "); //PowerUpDuration
00238         tok = strtok(NULL, " "); //TransmissionDuration
00239         tok = strtok(NULL, " "); //InputStatus
00240         tok = strtok(NULL, " "); //OutputStatus
00241         tok = strtok(NULL, " "); //ReservedByteA
00242         tok = strtok(NULL, " "); //ScanningFrequency
00243         tok = strtok(NULL, " "); //MeasurementFrequency
00244         tok = strtok(NULL, " ");
00245         tok = strtok(NULL, " ");
00246         tok = strtok(NULL, " ");
00247         tok = strtok(NULL, " "); //NumberEncoders
00248         int NumberEncoders;
00249         sscanf(tok, "%d", &NumberEncoders);
00250         for (int i = 0; i < NumberEncoders; i++) {
00251                 tok = strtok(NULL, " "); //EncoderPosition
00252                 tok = strtok(NULL, " "); //EncoderSpeed
00253         }
00254 
00255         tok = strtok(NULL, " "); //NumberChannels16Bit
00256         int NumberChannels16Bit;
00257         sscanf(tok, "%d", &NumberChannels16Bit);
00258         if (debug)
00259                 printf("NumberChannels16Bit : %d\n", NumberChannels16Bit);
00260         for (int i = 0; i < NumberChannels16Bit; i++) {
00261                 int type = -1; // 0 DIST1 1 DIST2 2 RSSI1 3 RSSI2
00262                 char content[6];
00263                 tok = strtok(NULL, " "); //MeasuredDataContent
00264                 sscanf(tok, "%s", content);
00265                 if (!strcmp(content, "DIST1")) {
00266                         type = 0;
00267                 } else if (!strcmp(content, "DIST2")) {
00268                         type = 1;
00269                 } else if (!strcmp(content, "RSSI1")) {
00270                         type = 2;
00271                 } else if (!strcmp(content, "RSSI2")) {
00272                         type = 3;
00273                 }
00274                 tok = strtok(NULL, " "); //ScalingFactor
00275                 tok = strtok(NULL, " "); //ScalingOffset
00276                 tok = strtok(NULL, " "); //Starting angle
00277                 tok = strtok(NULL, " "); //Angular step width
00278                 tok = strtok(NULL, " "); //NumberData
00279                 int NumberData;
00280                 sscanf(tok, "%X", &NumberData);
00281 
00282                 if (debug)
00283                         printf("NumberData : %d\n", NumberData);
00284 
00285                 if (type == 0) {
00286                         data.dist_len1 = NumberData;
00287                 } else if (type == 1) {
00288                         data.dist_len2 = NumberData;
00289                 } else if (type == 2) {
00290                         data.rssi_len1 = NumberData;
00291                 } else if (type == 3) {
00292                         data.rssi_len2 = NumberData;
00293                 }
00294 
00295                 for (int i = 0; i < NumberData; i++) {
00296                         int dat;
00297                         tok = strtok(NULL, " "); //data
00298                         sscanf(tok, "%X", &dat);
00299 
00300                         if (type == 0) {
00301                                 data.dist1[i] = dat;
00302                         } else if (type == 1) {
00303                                 data.dist2[i] = dat;
00304                         } else if (type == 2) {
00305                                 data.rssi1[i] = dat;
00306                         } else if (type == 3) {
00307                                 data.rssi2[i] = dat;
00308                         }
00309 
00310                 }
00311         }
00312 
00313         tok = strtok(NULL, " "); //NumberChannels8Bit
00314         int NumberChannels8Bit;
00315         sscanf(tok, "%d", &NumberChannels8Bit);
00316         if (debug)
00317                 printf("NumberChannels8Bit : %d\n", NumberChannels8Bit);
00318         for (int i = 0; i < NumberChannels8Bit; i++) {
00319                 int type = -1;
00320                 char content[6];
00321                 tok = strtok(NULL, " "); //MeasuredDataContent
00322                 sscanf(tok, "%s", content);
00323                 if (!strcmp(content, "DIST1")) {
00324                         type = 0;
00325                 } else if (!strcmp(content, "DIST2")) {
00326                         type = 1;
00327                 } else if (!strcmp(content, "RSSI1")) {
00328                         type = 2;
00329                 } else if (!strcmp(content, "RSSI2")) {
00330                         type = 3;
00331                 }
00332                 tok = strtok(NULL, " "); //ScalingFactor
00333                 tok = strtok(NULL, " "); //ScalingOffset
00334                 tok = strtok(NULL, " "); //Starting angle
00335                 tok = strtok(NULL, " "); //Angular step width
00336                 tok = strtok(NULL, " "); //NumberData
00337                 int NumberData;
00338                 sscanf(tok, "%X", &NumberData);
00339 
00340                 if (debug)
00341                         printf("NumberData : %d\n", NumberData);
00342 
00343                 if (type == 0) {
00344                         data.dist_len1 = NumberData;
00345                 } else if (type == 1) {
00346                         data.dist_len2 = NumberData;
00347                 } else if (type == 2) {
00348                         data.rssi_len1 = NumberData;
00349                 } else if (type == 3) {
00350                         data.rssi_len2 = NumberData;
00351                 }
00352                 for (int i = 0; i < NumberData; i++) {
00353                         int dat;
00354                         tok = strtok(NULL, " "); //data
00355                         sscanf(tok, "%X", &dat);
00356 
00357                         if (type == 0) {
00358                                 data.dist1[i] = dat;
00359                         } else if (type == 1) {
00360                                 data.dist2[i] = dat;
00361                         } else if (type == 2) {
00362                                 data.rssi1[i] = dat;
00363                         } else if (type == 3) {
00364                                 data.rssi2[i] = dat;
00365                         }
00366                 }
00367         }
00368 
00369 }
00370 
00371 void LMS1xx::saveConfig() {
00372         char buf[100];
00373         sprintf(buf, "%c%s%c", 0x02, "sMN mEEwriteall", 0x03);
00374 
00375         write(sockDesc, buf, strlen(buf));
00376 
00377         int len = read(sockDesc, buf, 100);
00378         //      if (buf[0] != 0x02)
00379         //              std::cout << "invalid packet recieved" << std::endl;
00380         //      if (debug) {
00381         //              buf[len] = 0;
00382         //              std::cout << buf << std::endl;
00383         //      }
00384 }
00385 
00386 void LMS1xx::startDevice() {
00387         char buf[100];
00388         sprintf(buf, "%c%s%c", 0x02, "sMN Run", 0x03);
00389 
00390         write(sockDesc, buf, strlen(buf));
00391 
00392         int len = read(sockDesc, buf, 100);
00393         //      if (buf[0] != 0x02)
00394         //              std::cout << "invalid packet recieved" << std::endl;
00395         //      if (debug) {
00396         //              buf[len] = 0;
00397         //              std::cout << buf << std::endl;
00398         //      }
00399 }