lms1xx.cpp
Go to the documentation of this file.
1 /*
2  * LMS1xx.cpp
3  *
4  * Created on: 09-08-2010
5  * Author: Konrad Banachowicz
6  ***************************************************************************
7  * This library is free software; you can redistribute it and/or *
8  * modify it under the terms of the GNU Lesser General Public *
9  * License as published by the Free Software Foundation; either *
10  * version 2.1 of the License, or (at your option) any later version. *
11  * *
12  * This library is distributed in the hope that it will be useful, *
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
15  * Lesser General Public License for more details. *
16  * *
17  * You should have received a copy of the GNU Lesser General Public *
18  * License along with this library; if not, write to the Free Software *
19  * Foundation, Inc., 59 Temple Place, *
20  * Suite 330, Boston, MA 02111-1307 USA *
21  * *
22  ***************************************************************************/
23 
24 #include <sys/types.h>
25 #include <sys/socket.h>
26 #include <netinet/in.h>
27 #include <arpa/inet.h>
28 #include <cstdio>
29 #include <cstdlib>
30 #include <cstring>
31 #include <unistd.h>
32 
33 #include <lms1xx/lms1xx.h>
34 
35 LMS1xx::LMS1xx() :
36  connected(false) {
37  debug = false;
38 }
39 
40 LMS1xx::~LMS1xx() {
41 
42 }
43 
44 void LMS1xx::connect(std::string host, int port) {
45  if (!connected) {
46  sockDesc = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
47  if (sockDesc) {
48  struct sockaddr_in stSockAddr;
49  int Res;
50  stSockAddr.sin_family = PF_INET;
51  stSockAddr.sin_port = htons(port);
52  Res = inet_pton(AF_INET, host.c_str(), &stSockAddr.sin_addr);
53 
54  int ret = ::connect(sockDesc, (struct sockaddr *) &stSockAddr,
55  sizeof stSockAddr);
56  if (ret == 0) {
57  connected = true;
58  }
59  }
60  }
61 }
62 
63 void LMS1xx::disconnect() {
64  if (connected) {
65  close(sockDesc);
66  connected = false;
67  }
68 }
69 
70 bool LMS1xx::isConnected() {
71  return connected;
72 }
73 
74 void LMS1xx::startMeas() {
75  char buf[100];
76  sprintf(buf, "%c%s%c", 0x02, "sMN LMCstartmeas", 0x03);
77 
78  write(sockDesc, buf, strlen(buf));
79 
80  int len = read(sockDesc, buf, 100);
81  // if (buf[0] != 0x02)
82  // std::cout << "invalid packet recieved" << std::endl;
83  // if (debug) {
84  // buf[len] = 0;
85  // std::cout << buf << std::endl;
86  // }
87 }
88 
89 void LMS1xx::stopMeas() {
90  char buf[100];
91  sprintf(buf, "%c%s%c", 0x02, "sMN LMCstopmeas", 0x03);
92 
93  write(sockDesc, buf, strlen(buf));
94 
95  int len = read(sockDesc, buf, 100);
96  // if (buf[0] != 0x02)
97  // std::cout << "invalid packet recieved" << std::endl;
98  // if (debug) {
99  // buf[len] = 0;
100  // std::cout << buf << std::endl;
101  // }
102 }
103 
104 status_t LMS1xx::queryStatus() {
105  char buf[100];
106  sprintf(buf, "%c%s%c", 0x02, "sRN STlms", 0x03);
107 
108  write(sockDesc, buf, strlen(buf));
109 
110  int len = read(sockDesc, buf, 100);
111  // if (buf[0] != 0x02)
112  // std::cout << "invalid packet recieved" << std::endl;
113  // if (debug) {
114  // buf[len] = 0;
115  // std::cout << buf << std::endl;
116  // }
117  int ret;
118  sscanf((buf + 10), "%d", &ret);
119 
120  return (status_t) ret;
121 }
122 
123 void LMS1xx::login() {
124  char buf[100];
125  sprintf(buf, "%c%s%c", 0x02, "sMN SetAccessMode 03 F4724744", 0x03);
126 
127  write(sockDesc, buf, strlen(buf));
128 
129  int len = read(sockDesc, buf, 100);
130  // if (buf[0] != 0x02)
131  // std::cout << "invalid packet recieved" << std::endl;
132  // if (debug) {
133  // buf[len] = 0;
134  // std::cout << buf << std::endl;
135  // }
136 }
137 
138 scanCfg LMS1xx::getScanCfg() const {
139  scanCfg cfg;
140  char buf[100];
141  sprintf(buf, "%c%s%c", 0x02, "sRN LMPscancfg", 0x03);
142 
143  write(sockDesc, buf, strlen(buf));
144 
145  int len = read(sockDesc, buf, 100);
146  // if (buf[0] != 0x02)
147  // std::cout << "invalid packet recieved" << std::endl;
148  // if (debug) {
149  // buf[len] = 0;
150  // std::cout << buf << std::endl;
151  // }
152 
153  sscanf(buf + 1, "%*s %*s %X %*d %X %X %X", &cfg.scaningFrequency,
154  &cfg.angleResolution, &cfg.startAngle, &cfg.stopAngle);
155  return cfg;
156 }
157 
158 void LMS1xx::setScanCfg(const scanCfg &cfg) {
159  char buf[100];
160  sprintf(buf, "%c%s %X +1 %X %X %X%c", 0x02, "sMN mLMPsetscancfg",
161  cfg.scaningFrequency, cfg.angleResolution, cfg.startAngle,
162  cfg.stopAngle, 0x03);
163 
164  write(sockDesc, buf, strlen(buf));
165 
166  int len = read(sockDesc, buf, 100);
167 
168  buf[len - 1] = 0;
169 }
170 
171 void LMS1xx::setScanDataCfg(const scanDataCfg &cfg) {
172  char buf[100];
173  sprintf(buf, "%c%s %02X 00 %d %d 0 %02X 00 %d %d 0 %d +%d%c", 0x02,
174  "sWN LMDscandatacfg", cfg.outputChannel, cfg.remission ? 1 : 0,
175  cfg.resolution, cfg.encoder, cfg.position ? 1 : 0,
176  cfg.deviceName ? 1 : 0, cfg.timestamp ? 1 : 0, cfg.outputInterval, 0x03);
177  if(debug)
178  printf("%s\n", buf);
179  write(sockDesc, buf, strlen(buf));
180 
181  int len = read(sockDesc, buf, 100);
182  buf[len - 1] = 0;
183 }
184 
185 void LMS1xx::scanContinous(int start) {
186  char buf[100];
187  sprintf(buf, "%c%s %d%c", 0x02, "sEN LMDscandata", start, 0x03);
188 
189  write(sockDesc, buf, strlen(buf));
190 
191  int len = read(sockDesc, buf, 100);
192 
193  if (buf[0] != 0x02)
194  printf("invalid packet recieved\n");
195 
196  if (debug) {
197  buf[len] = 0;
198  printf("%s\n", buf);
199  }
200 
201  if (start = 0) {
202  for (int i = 0; i < 10; i++)
203  read(sockDesc, buf, 100);
204  }
205 }
206 
207 void LMS1xx::getData(scanData& data) {
208  char buf[20000];
209  fd_set rfds;
210  struct timeval tv;
211  int retval, len;
212  len = 0;
213 
214  do {
215  FD_ZERO(&rfds);
216  FD_SET(sockDesc, &rfds);
217 
218  tv.tv_sec = 0;
219  tv.tv_usec = 50000;
220  retval = select(sockDesc + 1, &rfds, NULL, NULL, &tv);
221  if (retval) {
222  len += read(sockDesc, buf + len, 20000 - len);
223  }
224  } while ((buf[0] != 0x02) || (buf[len - 1] != 0x03));
225 
226  // if (debug)
227  // std::cout << "scan data recieved" << std::endl;
228  buf[len - 1] = 0;
229  char* tok = strtok(buf, " "); //Type of command
230  tok = strtok(NULL, " "); //Command
231  tok = strtok(NULL, " "); //VersionNumber
232  tok = strtok(NULL, " "); //DeviceNumber
233  tok = strtok(NULL, " "); //Serial number
234  tok = strtok(NULL, " "); //DeviceStatus
235  tok = strtok(NULL, " "); //MessageCounter
236  tok = strtok(NULL, " "); //ScanCounter
237  tok = strtok(NULL, " "); //PowerUpDuration
238  tok = strtok(NULL, " "); //TransmissionDuration
239  tok = strtok(NULL, " "); //InputStatus
240  tok = strtok(NULL, " "); //OutputStatus
241  tok = strtok(NULL, " "); //ReservedByteA
242  tok = strtok(NULL, " "); //ScanningFrequency
243  tok = strtok(NULL, " "); //MeasurementFrequency
244  tok = strtok(NULL, " ");
245  tok = strtok(NULL, " ");
246  tok = strtok(NULL, " ");
247  tok = strtok(NULL, " "); //NumberEncoders
248  int NumberEncoders;
249  sscanf(tok, "%d", &NumberEncoders);
250  for (int i = 0; i < NumberEncoders; i++) {
251  tok = strtok(NULL, " "); //EncoderPosition
252  tok = strtok(NULL, " "); //EncoderSpeed
253  }
254 
255  tok = strtok(NULL, " "); //NumberChannels16Bit
256  int NumberChannels16Bit;
257  sscanf(tok, "%d", &NumberChannels16Bit);
258  if (debug)
259  printf("NumberChannels16Bit : %d\n", NumberChannels16Bit);
260  for (int i = 0; i < NumberChannels16Bit; i++) {
261  int type = -1; // 0 DIST1 1 DIST2 2 RSSI1 3 RSSI2
262  char content[6];
263  tok = strtok(NULL, " "); //MeasuredDataContent
264  sscanf(tok, "%s", content);
265  if (!strcmp(content, "DIST1")) {
266  type = 0;
267  } else if (!strcmp(content, "DIST2")) {
268  type = 1;
269  } else if (!strcmp(content, "RSSI1")) {
270  type = 2;
271  } else if (!strcmp(content, "RSSI2")) {
272  type = 3;
273  }
274  tok = strtok(NULL, " "); //ScalingFactor
275  tok = strtok(NULL, " "); //ScalingOffset
276  tok = strtok(NULL, " "); //Starting angle
277  tok = strtok(NULL, " "); //Angular step width
278  tok = strtok(NULL, " "); //NumberData
279  int NumberData;
280  sscanf(tok, "%X", &NumberData);
281 
282  if (debug)
283  printf("NumberData : %d\n", NumberData);
284 
285  if (type == 0) {
286  data.dist_len1 = NumberData;
287  } else if (type == 1) {
288  data.dist_len2 = NumberData;
289  } else if (type == 2) {
290  data.rssi_len1 = NumberData;
291  } else if (type == 3) {
292  data.rssi_len2 = NumberData;
293  }
294 
295  for (int i = 0; i < NumberData; i++) {
296  int dat;
297  tok = strtok(NULL, " "); //data
298  sscanf(tok, "%X", &dat);
299 
300  if (type == 0) {
301  data.dist1[i] = dat;
302  } else if (type == 1) {
303  data.dist2[i] = dat;
304  } else if (type == 2) {
305  data.rssi1[i] = dat;
306  } else if (type == 3) {
307  data.rssi2[i] = dat;
308  }
309 
310  }
311  }
312 
313  tok = strtok(NULL, " "); //NumberChannels8Bit
314  int NumberChannels8Bit;
315  sscanf(tok, "%d", &NumberChannels8Bit);
316  if (debug)
317  printf("NumberChannels8Bit : %d\n", NumberChannels8Bit);
318  for (int i = 0; i < NumberChannels8Bit; i++) {
319  int type = -1;
320  char content[6];
321  tok = strtok(NULL, " "); //MeasuredDataContent
322  sscanf(tok, "%s", content);
323  if (!strcmp(content, "DIST1")) {
324  type = 0;
325  } else if (!strcmp(content, "DIST2")) {
326  type = 1;
327  } else if (!strcmp(content, "RSSI1")) {
328  type = 2;
329  } else if (!strcmp(content, "RSSI2")) {
330  type = 3;
331  }
332  tok = strtok(NULL, " "); //ScalingFactor
333  tok = strtok(NULL, " "); //ScalingOffset
334  tok = strtok(NULL, " "); //Starting angle
335  tok = strtok(NULL, " "); //Angular step width
336  tok = strtok(NULL, " "); //NumberData
337  int NumberData;
338  sscanf(tok, "%X", &NumberData);
339 
340  if (debug)
341  printf("NumberData : %d\n", NumberData);
342 
343  if (type == 0) {
344  data.dist_len1 = NumberData;
345  } else if (type == 1) {
346  data.dist_len2 = NumberData;
347  } else if (type == 2) {
348  data.rssi_len1 = NumberData;
349  } else if (type == 3) {
350  data.rssi_len2 = NumberData;
351  }
352  for (int i = 0; i < NumberData; i++) {
353  int dat;
354  tok = strtok(NULL, " "); //data
355  sscanf(tok, "%X", &dat);
356 
357  if (type == 0) {
358  data.dist1[i] = dat;
359  } else if (type == 1) {
360  data.dist2[i] = dat;
361  } else if (type == 2) {
362  data.rssi1[i] = dat;
363  } else if (type == 3) {
364  data.rssi2[i] = dat;
365  }
366  }
367  }
368 
369 }
370 
371 void LMS1xx::saveConfig() {
372  char buf[100];
373  sprintf(buf, "%c%s%c", 0x02, "sMN mEEwriteall", 0x03);
374 
375  write(sockDesc, buf, strlen(buf));
376 
377  int len = read(sockDesc, buf, 100);
378  // if (buf[0] != 0x02)
379  // std::cout << "invalid packet recieved" << std::endl;
380  // if (debug) {
381  // buf[len] = 0;
382  // std::cout << buf << std::endl;
383  // }
384 }
385 
386 void LMS1xx::startDevice() {
387  char buf[100];
388  sprintf(buf, "%c%s%c", 0x02, "sMN Run", 0x03);
389 
390  write(sockDesc, buf, strlen(buf));
391 
392  int len = read(sockDesc, buf, 100);
393  // if (buf[0] != 0x02)
394  // std::cout << "invalid packet recieved" << std::endl;
395  // if (debug) {
396  // buf[len] = 0;
397  // std::cout << buf << std::endl;
398  // }
399 }
status_t
status_t
Definition: lms1xx.h:177
LMS1xx::debug
bool debug
Definition: lms1xx.h:300
LMS1xx::setScanDataCfg
void setScanDataCfg(const scanDataCfg &cfg)
Set scan data configuration. Set format of scan message returned by device.
Definition: lms1xx.cpp:171
LMS1xx::getScanCfg
scanCfg getScanCfg() const
Get current scan configuration. Get scan configuration :
Definition: lms1xx.cpp:138
LMS1xx::queryStatus
status_t queryStatus()
Get current status of LMS1xx device.
Definition: lms1xx.cpp:104
LMS1xx::login
void login()
Log into LMS1xx unit. Increase privilege level, giving ability to change device configuration.
Definition: lms1xx.cpp:123
LMS1xx::stopMeas
void stopMeas()
Stop measurements. After receiving this command LMS1xx unit stop spinning laser and measuring...
Definition: lms1xx.cpp:89
scanData
Structure containing single scan message.
scanCfg
Structure containing scan configuration.
LMS1xx::setScanCfg
void setScanCfg(const scanCfg &cfg)
Set scan configuration. Get scan configuration :
Definition: lms1xx.cpp:158
LMS1xx::startDevice
void startDevice()
The device is returned to the measurement mode after configuration.
Definition: lms1xx.cpp:386
LMS1xx::scanContinous
void scanContinous(int start)
Start or stop continuous data acquisition. After reception of this command device start or stop conti...
Definition: lms1xx.cpp:185
LMS1xx::connected
bool connected
Definition: lms1xx.h:299
LMS1xx::startMeas
void startMeas()
Start measurements. After receiving this command LMS1xx unit starts spinning laser and measuring...
Definition: lms1xx.cpp:74
LMS1xx::isConnected
bool isConnected()
Get status of connection.
Definition: lms1xx.cpp:70
LMS1xx::disconnect
void disconnect()
Disconnect from LMS1xx device.
Definition: lms1xx.cpp:63
LMS1xx::connect
void connect(std::string host, int port=2111)
Connect to LMS1xx.
Definition: lms1xx.cpp:44
LMS1xx::getData
void getData(scanData &data)
Receive single scan message.
Definition: lms1xx.cpp:207
LMS1xx::LMS1xx
LMS1xx()
Definition: lms1xx.cpp:35
LMS1xx::saveConfig
void saveConfig()
Save data permanently. Parameters are saved in the EEPROM of the LMS and will also be available after...
Definition: lms1xx.cpp:371
scanDataCfg
Structure containing scan data configuration.
LMS1xx::sockDesc
int sockDesc
Definition: lms1xx.h:302
LMS1xx::~LMS1xx
virtual ~LMS1xx()
Definition: lms1xx.cpp:40

lms1xx
Author(s): Konrad Banachowicz
autogenerated on Mon Mar 2 2015 01:32:13