lms1xx: 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/time.h>
00025 #include <sys/types.h>
00026 #include <sys/socket.h>
00027 #include <netinet/in.h>
00028 #include <arpa/inet.h>
00029 #include <cstdio>
00030 #include <cstdlib>
00031 #include <cstring>
00032 #include <unistd.h>
00033 
00034 #include "LMS1xx/LMS1xx.h"
00035 #include "console_bridge/console.h"
00036 
00037 LMS1xx::LMS1xx() :
00038         connected(false) {
00039 }
00040 
00041 LMS1xx::~LMS1xx() {
00042 
00043 }
00044 
00045 void LMS1xx::connect(std::string host, int port) {
00046         if (!connected) {
00047                 sockDesc = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
00048                 if (sockDesc) {
00049                         struct sockaddr_in stSockAddr;
00050                         int Res;
00051                         stSockAddr.sin_family = PF_INET;
00052                         stSockAddr.sin_port = htons(port);
00053                         Res = inet_pton(AF_INET, host.c_str(), &stSockAddr.sin_addr);
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                 logWarn("invalid packet recieved");
00083         buf[len] = 0;
00084         logDebug("RX: %s", buf);
00085 }
00086 
00087 void LMS1xx::stopMeas() {
00088         char buf[100];
00089         sprintf(buf, "%c%s%c", 0x02, "sMN LMCstopmeas", 0x03);
00090 
00091         write(sockDesc, buf, strlen(buf));
00092 
00093         int len = read(sockDesc, buf, 100);
00094         if (buf[0] != 0x02)
00095                 logWarn("invalid packet recieved");
00096         buf[len] = 0;
00097         logDebug("RX: %s", buf);
00098 }
00099 
00100 status_t LMS1xx::queryStatus() {
00101         char buf[100];
00102         sprintf(buf, "%c%s%c", 0x02, "sRN STlms", 0x03);
00103 
00104         write(sockDesc, buf, strlen(buf));
00105 
00106         int len = read(sockDesc, buf, 100);
00107         if (buf[0] != 0x02)
00108                 logWarn("invalid packet recieved");
00109         buf[len] = 0;
00110         logDebug("RX: %s", buf);
00111 
00112         int ret;
00113         sscanf((buf + 10), "%d", &ret);
00114 
00115         return (status_t) ret;
00116 }
00117 
00118 void LMS1xx::login() {
00119         char buf[100];
00120         int result;
00121         sprintf(buf, "%c%s%c", 0x02, "sMN SetAccessMode 03 F4724744", 0x03);
00122         
00123         fd_set readset;
00124         struct timeval timeout;
00125 
00126 
00127         do { //loop until data is available to read
00128           timeout.tv_sec = 1;
00129           timeout.tv_usec = 0;
00130 
00131           write(sockDesc, buf, strlen(buf));
00132 
00133           FD_ZERO(&readset);
00134           FD_SET(sockDesc, &readset);
00135           result = select(sockDesc + 1, &readset, NULL, NULL, &timeout);
00136 
00137         } while (result <= 0);
00138 
00139         int len = read(sockDesc, buf, 100);
00140         if (buf[0] != 0x02)
00141                 logWarn("invalid packet recieved");
00142         buf[len] = 0;
00143         logDebug("RX: %s", buf);
00144 }
00145 
00146 scanCfg LMS1xx::getScanCfg() const {
00147         scanCfg cfg;
00148         char buf[100];
00149         sprintf(buf, "%c%s%c", 0x02, "sRN LMPscancfg", 0x03);
00150 
00151         write(sockDesc, buf, strlen(buf));
00152 
00153         int len = read(sockDesc, buf, 100);
00154         if (buf[0] != 0x02)
00155                 logWarn("invalid packet recieved");
00156         buf[len] = 0;
00157         logDebug("RX: %s", buf);
00158 
00159         sscanf(buf + 1, "%*s %*s %X %*d %X %X %X", &cfg.scaningFrequency,
00160                         &cfg.angleResolution, &cfg.startAngle, &cfg.stopAngle);
00161         return cfg;
00162 }
00163 
00164 void LMS1xx::setScanCfg(const scanCfg &cfg) {
00165         char buf[100];
00166         sprintf(buf, "%c%s %X +1 %X %X %X%c", 0x02, "sMN mLMPsetscancfg",
00167                         cfg.scaningFrequency, cfg.angleResolution, cfg.startAngle,
00168                         cfg.stopAngle, 0x03);
00169 
00170         write(sockDesc, buf, strlen(buf));
00171 
00172         int len = read(sockDesc, buf, 100);
00173 
00174         buf[len - 1] = 0;
00175 }
00176 
00177 void LMS1xx::setScanDataCfg(const scanDataCfg &cfg) {
00178         char buf[100];
00179         sprintf(buf, "%c%s %02X 00 %d %d 0 %02X 00 %d %d 0 %d +%d%c", 0x02,
00180                         "sWN LMDscandatacfg", cfg.outputChannel, cfg.remission ? 1 : 0,
00181                         cfg.resolution, cfg.encoder, cfg.position ? 1 : 0,
00182                         cfg.deviceName ? 1 : 0, cfg.timestamp ? 1 : 0, cfg.outputInterval, 0x03);
00183         logDebug("TX: %s", buf);
00184         write(sockDesc, buf, strlen(buf));
00185 
00186         int len = read(sockDesc, buf, 100);
00187         buf[len - 1] = 0;
00188 }
00189 
00190 scanOutputRange LMS1xx::getScanOutputRange() const {
00191         scanOutputRange outputRange;
00192         char buf[100];
00193         sprintf(buf, "%c%s%c", 0x02, "sRN LMPoutputRange", 0x03);
00194 
00195         write(sockDesc, buf, strlen(buf));
00196 
00197         int len = read(sockDesc, buf, 100);
00198 
00199         sscanf(buf + 1, "%*s %*s %*d %X %X %X", &outputRange.angleResolution,
00200                 &outputRange.startAngle, &outputRange.stopAngle);
00201         return outputRange;
00202 }
00203 
00204 void LMS1xx::scanContinous(int start) {
00205         char buf[100];
00206         sprintf(buf, "%c%s %d%c", 0x02, "sEN LMDscandata", start, 0x03);
00207 
00208         write(sockDesc, buf, strlen(buf));
00209 
00210         int len = read(sockDesc, buf, 100);
00211 
00212         if (buf[0] != 0x02)
00213                 logError("invalid packet recieved");
00214 
00215         buf[len] = 0;
00216         logDebug("RX: %s", buf);
00217 }
00218 
00219 void LMS1xx::getData(scanData& data) {
00220         char buf[20000];
00221         fd_set rfds;
00222         struct timeval tv;
00223         int retval, len;
00224         len = 0;
00225 
00226         do {
00227                 FD_ZERO(&rfds);
00228                 FD_SET(sockDesc, &rfds);
00229 
00230                 tv.tv_sec = 0;
00231                 tv.tv_usec = 50000;
00232                 retval = select(sockDesc + 1, &rfds, NULL, NULL, &tv);
00233                 if (retval) {
00234                         len += read(sockDesc, buf + len, 20000 - len);
00235                 }
00236         } while ((buf[0] != 0x02) || (buf[len - 1] != 0x03));
00237 
00238         logDebug("scan data recieved");
00239         buf[len - 1] = 0;
00240         char* tok = strtok(buf, " "); //Type of command
00241         tok = strtok(NULL, " "); //Command
00242         tok = strtok(NULL, " "); //VersionNumber
00243         tok = strtok(NULL, " "); //DeviceNumber
00244         tok = strtok(NULL, " "); //Serial number
00245         tok = strtok(NULL, " "); //DeviceStatus
00246         tok = strtok(NULL, " "); //MessageCounter
00247         tok = strtok(NULL, " "); //ScanCounter
00248         tok = strtok(NULL, " "); //PowerUpDuration
00249         tok = strtok(NULL, " "); //TransmissionDuration
00250         tok = strtok(NULL, " "); //InputStatus
00251         tok = strtok(NULL, " "); //OutputStatus
00252         tok = strtok(NULL, " "); //ReservedByteA
00253         tok = strtok(NULL, " "); //ScanningFrequency
00254         tok = strtok(NULL, " "); //MeasurementFrequency
00255         tok = strtok(NULL, " ");
00256         tok = strtok(NULL, " ");
00257         tok = strtok(NULL, " ");
00258         tok = strtok(NULL, " "); //NumberEncoders
00259         int NumberEncoders;
00260         sscanf(tok, "%d", &NumberEncoders);
00261         for (int i = 0; i < NumberEncoders; i++) {
00262                 tok = strtok(NULL, " "); //EncoderPosition
00263                 tok = strtok(NULL, " "); //EncoderSpeed
00264         }
00265 
00266         tok = strtok(NULL, " "); //NumberChannels16Bit
00267         int NumberChannels16Bit;
00268         sscanf(tok, "%d", &NumberChannels16Bit);
00269         logDebug("NumberChannels16Bit : %d", NumberChannels16Bit);
00270 
00271         for (int i = 0; i < NumberChannels16Bit; i++) {
00272                 int type = -1; // 0 DIST1 1 DIST2 2 RSSI1 3 RSSI2
00273                 char content[6];
00274                 tok = strtok(NULL, " "); //MeasuredDataContent
00275                 sscanf(tok, "%s", content);
00276                 if (!strcmp(content, "DIST1")) {
00277                         type = 0;
00278                 } else if (!strcmp(content, "DIST2")) {
00279                         type = 1;
00280                 } else if (!strcmp(content, "RSSI1")) {
00281                         type = 2;
00282                 } else if (!strcmp(content, "RSSI2")) {
00283                         type = 3;
00284                 }
00285                 tok = strtok(NULL, " "); //ScalingFactor
00286                 tok = strtok(NULL, " "); //ScalingOffset
00287                 tok = strtok(NULL, " "); //Starting angle
00288                 tok = strtok(NULL, " "); //Angular step width
00289                 tok = strtok(NULL, " "); //NumberData
00290                 int NumberData;
00291                 sscanf(tok, "%X", &NumberData);
00292                 logDebug("NumberData : %d", NumberData);
00293 
00294                 if (type == 0) {
00295                         data.dist_len1 = NumberData;
00296                 } else if (type == 1) {
00297                         data.dist_len2 = NumberData;
00298                 } else if (type == 2) {
00299                         data.rssi_len1 = NumberData;
00300                 } else if (type == 3) {
00301                         data.rssi_len2 = NumberData;
00302                 }
00303 
00304                 for (int i = 0; i < NumberData; i++) {
00305                         int dat;
00306                         tok = strtok(NULL, " "); //data
00307                         sscanf(tok, "%X", &dat);
00308 
00309                         if (type == 0) {
00310                                 data.dist1[i] = dat;
00311                         } else if (type == 1) {
00312                                 data.dist2[i] = dat;
00313                         } else if (type == 2) {
00314                                 data.rssi1[i] = dat;
00315                         } else if (type == 3) {
00316                                 data.rssi2[i] = dat;
00317                         }
00318 
00319                 }
00320         }
00321 
00322         tok = strtok(NULL, " "); //NumberChannels8Bit
00323         int NumberChannels8Bit;
00324         sscanf(tok, "%d", &NumberChannels8Bit);
00325         logDebug("NumberChannels8Bit : %d\n", NumberChannels8Bit);
00326 
00327         for (int i = 0; i < NumberChannels8Bit; i++) {
00328                 int type = -1;
00329                 char content[6];
00330                 tok = strtok(NULL, " "); //MeasuredDataContent
00331                 sscanf(tok, "%s", content);
00332                 if (!strcmp(content, "DIST1")) {
00333                         type = 0;
00334                 } else if (!strcmp(content, "DIST2")) {
00335                         type = 1;
00336                 } else if (!strcmp(content, "RSSI1")) {
00337                         type = 2;
00338                 } else if (!strcmp(content, "RSSI2")) {
00339                         type = 3;
00340                 }
00341                 tok = strtok(NULL, " "); //ScalingFactor
00342                 tok = strtok(NULL, " "); //ScalingOffset
00343                 tok = strtok(NULL, " "); //Starting angle
00344                 tok = strtok(NULL, " "); //Angular step width
00345                 tok = strtok(NULL, " "); //NumberData
00346                 int NumberData;
00347                 sscanf(tok, "%X", &NumberData);
00348                 logDebug("NumberData : %d\n", NumberData);
00349 
00350                 if (type == 0) {
00351                         data.dist_len1 = NumberData;
00352                 } else if (type == 1) {
00353                         data.dist_len2 = NumberData;
00354                 } else if (type == 2) {
00355                         data.rssi_len1 = NumberData;
00356                 } else if (type == 3) {
00357                         data.rssi_len2 = NumberData;
00358                 }
00359                 for (int i = 0; i < NumberData; i++) {
00360                         int dat;
00361                         tok = strtok(NULL, " "); //data
00362                         sscanf(tok, "%X", &dat);
00363 
00364                         if (type == 0) {
00365                                 data.dist1[i] = dat;
00366                         } else if (type == 1) {
00367                                 data.dist2[i] = dat;
00368                         } else if (type == 2) {
00369                                 data.rssi1[i] = dat;
00370                         } else if (type == 3) {
00371                                 data.rssi2[i] = dat;
00372                         }
00373                 }
00374         }
00375 
00376 }
00377 
00378 void LMS1xx::saveConfig() {
00379         char buf[100];
00380         sprintf(buf, "%c%s%c", 0x02, "sMN mEEwriteall", 0x03);
00381 
00382         write(sockDesc, buf, strlen(buf));
00383 
00384         int len = read(sockDesc, buf, 100);
00385 
00386         if (buf[0] != 0x02)
00387                 logWarn("invalid packet recieved");
00388         buf[len] = 0;
00389         logDebug("RX: %s", buf);
00390 }
00391 
00392 void LMS1xx::startDevice() {
00393         char buf[100];
00394         sprintf(buf, "%c%s%c", 0x02, "sMN Run", 0x03);
00395 
00396         write(sockDesc, buf, strlen(buf));
00397 
00398         int len = read(sockDesc, buf, 100);
00399 
00400         if (buf[0] != 0x02)
00401                 logWarn("invalid packet recieved");
00402         buf[len] = 0;
00403         logDebug("RX: %s", buf);
00404 }