iCub-main
can_string_generic.h
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 /*
4  * Copyright (C) 2008 The RobotCub Consortium
5  * Author: Marco Randazzo, Marco Maggiali
6  * website: www.robotcub.org
7  * Permission is granted to copy, distribute, and/or modify this program
8  * under the terms of the GNU General Public License, version 2 or any
9  * later version published by the Free Software Foundation.
10  *
11  * A copy of the license can be found at
12  * http://www.robotcub.org/icub/license/gpl.txt
13  *
14  * This program is distributed in the hope that it will be useful, but
15  * WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
17  * Public License for more details
18 */
19 
20 #ifndef __can_string_generic__
21 #define __can_string_generic__
22 
23 #include "messages.h"
24 
25 #include <yarp/dev/CanBusInterface.h>
26 #include <string.h>
27 #include <yarp/os/LogStream.h>
28 #include <yarp/os/Log.h>
29 #define MAX_STRINGS 4
30 
32 {
33 private:
34 
35 struct data_struct{
36  char text_buffer [256];
37  bool complete;
38  bool maybe_last_part;
39  char board_id;
40  int expected_length;
41  int current_length;
42 };
43  data_struct data[MAX_STRINGS];
44 
45 public:
49  inline can_string_generic();
50 
54  inline ~can_string_generic();
55 
62  inline int add_string(void* can_packet);
63 
71  inline char* print(int buffer_num, const char* canDevName, int netNum);
72 
77  inline void clear_string(int buffer_num);
78 };
79 
81 {
82  int j=0;
83  for (j=0; j<MAX_STRINGS; j++) clear_string(j);
84 }
85 
87 {
88 }
89 
91 {
92  if (buffer_num >= MAX_STRINGS) return ;
93 
94  for (int i = 0; i < 256; i++) data[buffer_num].text_buffer[i]=0;
95  data[buffer_num].complete=false;
96  data[buffer_num].maybe_last_part=false;
97  data[buffer_num].expected_length = 0;
98  data[buffer_num].board_id=0;
99  data[buffer_num].current_length = 0;
100 
101 }
102 
103 int can_string_generic::add_string(void* can_packet)
104 {
105  int j=0;
106  int string_id = 0;
107  int offset = 0;
108  yarp::dev::CanMessage* t;
109  unsigned int id=0;
110  unsigned char len=0;
111 
112  t = (yarp::dev::CanMessage*) (can_packet);
113  id=t->getId();
114  len=t->getLen();
115  unsigned char *candata=t->getData();
116  string_id = (candata[1]>>4);
117  offset = (candata[1]&0x0F);
118  data[string_id].board_id = char(id>>4&0xf);
119 
120  if (string_id>=MAX_STRINGS)
121  {
122  yError("msg from board %d contains an ERROR! (>MAX_STRINGS)\n",data[string_id ].board_id);
123  return -1;
124  }
125 
126  for (j=0 ; j<len-2; j++)
127  data[string_id].text_buffer[j+offset*6]=candata[j+2];
128 
129  if (candata[0]==ICUBCANPROTO_PER_MC_MSG__PRINT + 128)
130  {
131  data[string_id].maybe_last_part = true;
132  data[string_id].expected_length=offset*6+len-2;
133  }
134 
135  if (data[string_id].maybe_last_part)
136  {
137  data[string_id].current_length=strlen(data[string_id].text_buffer);
138 
139  if (data[string_id].expected_length==data[string_id].current_length)
140  data[string_id].complete = true; //check me
141  }
142 /*
143  if (data[string_id].complete)
144  {
145  print(string_id);
146  clear_string(string_id);
147  }
148 */
149 /*
150  //DEBUG ONLY
151  ACE_OS::printf("%d %d\r\n",string_id,offset);
152 
153  for (j=0 ; j<50; j++)
154  ACE_OS::printf("%c",data[string_id].text_buffer[j]);
155 
156  ACE_OS::printf("\r\n");
157 
158  for (j=0 ; j<50; j++)
159  ACE_OS::printf("%d",data[string_id].text_buffer[j]);
160 
161  ACE_OS::printf("\r\n");
162 */
163  if (data[string_id].complete) return string_id;
164  else return -1;
165 }
166 
167 char* can_string_generic::print(int buffer_num, const char* canDevName, int netNum)
168 {
169  if (buffer_num>=MAX_STRINGS) return 0;
170 
171  yDebug("%s [%d] msg from board %d: %s \n", canDevName, netNum, data[buffer_num].board_id , data[buffer_num].text_buffer);
172 
173  return data[buffer_num].text_buffer;
174 }
175 
176 
177 #endif
#define MAX_STRINGS
void clear_string(int buffer_num)
Resets the string buffer.
can_string_generic()
Default constructor.
char * print(int buffer_num, const char *canDevName, int netNum)
Prints a string buffer.
int add_string(void *can_packet)
Process a string can packet.
~can_string_generic()
Destructor.
degrees offset
Definition: sine.m:4