Solved

[C/C++] How to implement serial protocol with HEX data packet

Posted on 2014-01-21
54
1,571 Views
Last Modified: 2016-03-02
Hi to all,

I need to read data from an old sensor which use a serial port to send packets.
I think it would be better for me to use C/C++ under Ubuntu.

In the sensor manual, packet format is explained in this way:

Each data packet begins with a header byte (255) and end with a
checksum. The checksum is calculated in the following manner:
1. Sum all packet contents except header and checksum.
2. Divide the sum by 256.
3. The remainder should equal the checksum.

The header byte 0xFF will likely not be the only 0xFF byte in the data
packet. You must count the bytes received at your serial port and use the
checksum to ensure you are in sync with the data sent by the DMU. This is
especially critical when using the continuous data packet output mode.
I attached to this topic a table with all fields contained in a single packet.

I also have a matlab code which is able to read the packet, but I'm not able to understand what it does.

input='FFFFB30029FF86FF8A0009FFD5FFB22AC20632A48CCB';

if strcmp(input(1:2),'FF')
    input=input(3:end);
    s=length(input)-2;
    n=s/4;
for i=1:n
    a(i,1:4)=input(4*(i-1)+1:4*i);
end

for i=1:n
    str=a(i,:);
    %% Checksum
    %ch(i,1)=hex2dec(str);
if hex2dec(str(1))>=8
   o1(i,1)=hex2dec(str)-2^16;
else
   o1(i,1)=hex2dec(str);
end
end
o1

sfr=100*1.5/2^15;
sfa=2*1.5/2^15;
sfan=180/2^15;;

o1sc(1:2,1)=o1(1:2,1)*sfan; % (deg) Roll and Pitch 
o1sc(3:5,1)=o1(3:5,1)*sfr; %deg/s Tilt rate
o1sc(6:8,1)=o1(6:8,1)*sfa; % g accelerations
o1sc(9,1)=((o1(9,1) * 5/4096)-1.375)*44.44; % Temperature (8C)
 x = int16(o1(10,1));
  if x < 0
    y = uint16(double(x) + 65536);
  else
  y = uint16(x);
  end
o1sc(10,1)=double(y)*790*10^-9; 
o1sc


end

Open in new window


Can someone explain me what the matlab code do, please?
I do not know how to translate it in C code, unfortunately.
Any tips and suggestions are really appreciate!! :)

Thank you!
table.png
0
Comment
Question by:Marcus-Barnet
  • 24
  • 21
  • 9
54 Comments
 
LVL 86

Assisted Solution

by:jkr
jkr earned 250 total points
ID: 39797658
Including the serial comm part, that would be like

#include <stdio.h>   /* Standard input/output definitions */
 #include <string.h>  /* String function definitions */
 #include <unistd.h>  /* UNIX standard function definitions */
 #include <fcntl.h>   /* File control definitions */
 #include <errno.h>   /* Error number definitions */
 #include <termios.h> /* POSIX terminal control definitions */
#include <netinet/in.h>

 /*
  * 'open_port()' - Open serial port 1.
  *
  * Returns the file descriptor on success or -1 on error.
  */

 int open_port(void)
 {
   int fd;                                   /* File descriptor for the port */

   fd = open("/dev/ttyS1", O_RDWR | O_NOCTTY | O_NDELAY);

   if (fd == -1)
   {                                              /* Could not open the port */
     fprintf(stderr, "open_port: Unable to open /dev/ttyS1 - %s\n",
             strerror(errno));
   }

   return (fd);
 }

typedef unsigned char BYTE;

#pragma pack(push, 1) // save current pack setting and set to 1
struct IMU_data_packet /* all shorts are network byte order! */
{
  BYTE header;
  unsigned short roll;
  unsigned short pitch;
  unsigned short yaw;
  unsigned short accX;
  unsigned short accY;
  unsigned short accZ;
  unsigned short temp;
  unsigned short time;
  BYTE chksum;
};
#pragma pack(pop)     // return to previous pack setting

void process_packet(IMU_data_packet* p) {

  BYTE* buf = (BYTE*) p + 1;
  BYTE chksum = 0;
  for (unsigned int i = 0; i < 16; ++i) {  // calc. checksum

    chksum += buf[i];
  }

  if (chksum / 256 != p->chksum) {

     
     fprintf(stderr, "process_packet: packet checksum mismatch\n");

     return;
  }

  unsigned short roll = ntohs(p->roll);
  unsigned short pitch = ntohs(p->pitch);
  unsigned short yaw = ntohs(p->yaw);
  unsigned short accX = ntohs(p->accX);
  unsigned short accY = ntohs(p->accY);
  unsigned short accZ = ntohs(p->accZ);
  unsigned short temp = ntohs(p->temp);
  unsigned short time = ntohs(p->time);

  /* Now, do somthing with all that data! */
};

void main()
{
 int spfd=0;                                            /* File descriptor */
 char buf[18]; /* for one IMU data packet */
 struct termios options;
   
 spfd = open_port();

 fcntl(spfd, F_SETFL, FNDELAY);                  /* Configure port reading */
                                     /* Get the current options for the port */
 tcgetattr(spfd, &options);
 cfsetispeed(&options, B9600);                 /* Set the baud rates to 9600 */
 cfsetospeed(&options, B9600);
    
                                   /* Enable the receiver and set local mode */
 options.c_cflag |= (CLOCAL | CREAD);
 options.c_cflag &= ~PARENB; /* Mask the character size to 8 bits, no parity */
 options.c_cflag &= ~CSTOPB;
 options.c_cflag &= ~CSIZE;
 options.c_cflag |=  CS8;                              /* Select 8 data bits */
 options.c_cflag &= ~CRTSCTS;               /* Disable hardware flow control */  
 
                                 /* Enable data to be processed as raw input */
 options.c_lflag &= ~(ICANON | ECHO | ISIG);
       
                                        /* Set the new options for the port */
 tcsetattr(spfd, TCSANOW, &options);
                         
 while (1)
 {
   read(spfd, &buf, sizeof(buf));          /* Read packet */
   
   process_packet(IMU_data_packet*) buf);

   usleep(20000);
 }
                                                    /* Close the serial port */
  close(spfd);
 }

Open in new window

0
 
LVL 86

Expert Comment

by:jkr
ID: 39797663
Ooops, forgot - the serial comm code was taken from http://ftp.slackware.no/slackware/slackware-3.9/docs/mini/Serial-Port-Programming

The rest is - well, a diligent but routine piece of work ;o)
0
 

Author Comment

by:Marcus-Barnet
ID: 39797818
Hello jkr! You always save me! :)

I tried your code, I just had to change something in order to compile it with gcc under my Ubuntu distribution (ie. I added a typedef to the struct and change the baudrate).

When I run the code, I always have a checksum mismatch and I can't understand why.
I've better read the manual and I realized that the serial protocol sends data in binary format and not in hexadecimal.
In the manual, there is written:

In general, the digital data representing each measurement is sent as a 16-bit
number (two bytes). The data is sent MSB first then LSB.

I'm attaching the entire manual.
Moreover, the IMU has to different behavior:

if  you write G on the serial port, it only sends one packet;
if you write C on the serial port, then it continuously sends packet on the serial port.

At the moment, IMU sensor is on continuous mode and i'm trying to send G command without any success.

I put a printf statement in the while loop and I get only strange characters on the screen. :(

Can you help me, please?

#include <stdio.h>   /* Standard input/output definitions */
 #include <string.h>  /* String function definitions */
 #include <unistd.h>  /* UNIX standard function definitions */
 #include <fcntl.h>   /* File control definitions */
 #include <errno.h>   /* Error number definitions */
 #include <termios.h> /* POSIX terminal control definitions */
#include <netinet/in.h>

 /*
  * 'open_port()' - Open serial port 1.
  *
  * Returns the file descriptor on success or -1 on error.
  */

 int open_port(void)
 {
   int fd;                                   /* File descriptor for the port */

   fd = open("/dev/ttyUSB1", O_RDWR | O_NOCTTY | O_NDELAY);

   if (fd == -1)
   {                                              /* Could not open the port */
     fprintf(stderr, "open_port: Unable to open /dev/ttyS1 - %s\n",
             strerror(errno));
   }

   return (fd);
 }

typedef unsigned char BYTE;

#pragma pack(push, 1) // save current pack setting and set to 1
struct IMU_data_packet /* all shorts are network byte order! */
{
  BYTE header;
  unsigned short roll;
  unsigned short pitch;
  unsigned short yaw;
  unsigned short accX;
  unsigned short accY;
  unsigned short accZ;
  unsigned short temp;
  unsigned short time;
  BYTE chksum;
};
#pragma pack(pop)     // return to previous pack setting
 typedef struct IMU_data_packet packet;
void process_packet(packet* p) {
  unsigned int i = 0;
  BYTE* buf = (BYTE*) p + 1;
  BYTE chksum = 0;
  for (i = 0; i < 16; ++i) {  // calc. checksum

    chksum += buf[i];
  }
/*
  if (chksum / 256 != p->chksum) {

     
     fprintf(stderr, "process_packet: packet checksum mismatch\n");

     return;
  }
*/
  unsigned short roll = ntohs(p->roll);
  unsigned short pitch = ntohs(p->pitch);
  unsigned short yaw = ntohs(p->yaw);
  unsigned short accX = ntohs(p->accX);
  unsigned short accY = ntohs(p->accY);
  unsigned short accZ = ntohs(p->accZ);
  unsigned short temp = ntohs(p->temp);
  unsigned short time = ntohs(p->time);

  /* Now, do somthing with all that data! */
};

int main(void)
{
 int spfd=0;                                            /* File descriptor */
 char buf[18]; /* for one IMU data packet */
 struct termios options;
 
 spfd = open_port();

 fcntl(spfd, F_SETFL, FNDELAY);                  /* Configure port reading */
                                     /* Get the current options for the port */
 tcgetattr(spfd, &options);
 cfsetispeed(&options, B38400);                 /* Set the baud rates to 9600 */
 cfsetospeed(&options, B38400);
    
                                   /* Enable the receiver and set local mode */
 options.c_cflag |= (CLOCAL | CREAD);
 options.c_cflag &= ~PARENB; /* Mask the character size to 8 bits, no parity */
 options.c_cflag &= ~CSTOPB;
 options.c_cflag &= ~CSIZE;
 options.c_cflag |=  CS8;                              /* Select 8 data bits */
 options.c_cflag &= ~CRTSCTS;               /* Disable hardware flow control */  
 
                                 /* Enable data to be processed as raw input */
 options.c_lflag &= ~(ICANON | ECHO | ISIG);
       
                                        /* Set the new options for the port */
 tcsetattr(spfd, TCSANOW, &options);

char command='G';
write(spfd, &command, sizeof(command));  /* send G to switch to requested mode */
                      
 while (1)
 {   
   read(spfd, &buf, sizeof(buf));          /* Read packet */
   printf("%s", buf);  /* only print strange charactes */
   process_packet( (packet *) buf);

   usleep(200);
 }
                                                    /* Close the serial port */
  close(spfd);

return 0;
 }
                                            

Open in new window

0
 
LVL 86

Expert Comment

by:jkr
ID: 39797884
>>I've better read the manual and I realized that the serial protocol sends data in binary
>>format and not in hexadecimal.

That was what I assumend in the above, so that's nothing to worry about.

>>When I run the code, I always have a checksum mismatch and I can't understand why.

I can, my fault - the calculated checksum is incorrect because a single BYTE is not enough. Furthermore, I forgot to check the header. Try

void process_packet(IMU_data_packet* p) {

  BYTE* buf = (BYTE*) p + 1;
  unsigned int chksum = 0;

  if (p->header != 0xFF) {

     
     fprintf(stderr, "process_packet: packet header mismatch\n");

     return;
  }

  for (unsigned int i = 0; i < 16; ++i) {  // calc. checksum

    chksum += buf[i];
  }

  if (chksum / 256 != p->chksum) {

     
     fprintf(stderr, "process_packet: packet checksum mismatch\n");

     return;
  }

  unsigned short roll = ntohs(p->roll);
  unsigned short pitch = ntohs(p->pitch);
  unsigned short yaw = ntohs(p->yaw);
  unsigned short accX = ntohs(p->accX);
  unsigned short accY = ntohs(p->accY);
  unsigned short accZ = ntohs(p->accZ);
  unsigned short temp = ntohs(p->temp);
  unsigned short time = ntohs(p->time);

  /* Now, do somthing with all that data! */
};

Open in new window

0
 
LVL 86

Expert Comment

by:jkr
ID: 39797895
Argn, something else I forgot about::

>>3. The remainder should equal the checksum.

So that should be

void process_packet(IMU_data_packet* p) {

  BYTE* buf = (BYTE*) p + 1;
  unsigned int chksum = 0;

  if (p->header != 0xFF) {

     fprintf(stderr, "process_packet: packet header mismatch\n");

     return;
  }

  for (unsigned int i = 0; i < 16; ++i) {  // calc. checksum

    chksum += buf[i];
  }

  if (chksum % 256 != p->chksum) {

     fprintf(stderr, "process_packet: packet checksum mismatch\n");

     return;
  }

  unsigned short roll = ntohs(p->roll);
  unsigned short pitch = ntohs(p->pitch);
  unsigned short yaw = ntohs(p->yaw);
  unsigned short accX = ntohs(p->accX);
  unsigned short accY = ntohs(p->accY);
  unsigned short accZ = ntohs(p->accZ);
  unsigned short temp = ntohs(p->temp);
  unsigned short time = ntohs(p->time);

  /* Now, do somthing with all that data! */
}

Open in new window


instead...
0
 

Author Comment

by:Marcus-Barnet
ID: 39797938
Thank you again, JKR!

I tried the code, but now it gives me the header mismatch error.
I added a simple printf statement in the while loop in order to print HEX values (may be it is wrong, but it seems to print real hex values) and this is the output:

73 00 5a ff fc 00 00 ec 25 ed ba 00 25 2a cd 06 70 7f 55 e8 ff 00 4e 00 64 00 5c 00 00 ec 25 ed b7 00 25 2a c6 06 70 67 82 39 ff 00 86 00 00 00 55 00 00 ec 25 ed ba 00 29 2a c6 06 70 4f b3 26 ff ff c4 00 24 00 1c 00 00 ec 25 ed b3 00 33 2a c6 06 70 37 e2 68 ff 00 2f 00 99 00 4f 00 00 ec 25 ed b7 00 33 2a d0 06 70 20 11 a2 ff ff 77 ff e2 00 3d 00 00 ec 25 ed ba 00 36 2a cd 06 70 08 3d 36 ff 00 54 00 85 00 3b 00 00 ec 25 ed b3 00 33 2a d0 06 70 f0 6e c8 ff 01 0d 00 9d ff df 00 00 ec 25 ed ba 00 33 2a d3 06 70 d8 99 5a ff 00 4d 00 1a 00 4b 00 00 ec 25 ed ba 00 33 2a cd 06 70 c0 c2 8e ff 00 4d ff 85 00 76 00 00 ec 25 ed b7 00 2f 2a d0 06 70 a8 f0 35 ff 00 86 ff 74 00 fc 00 00 ec 25 ed ba 00 2c 2a d0 06 70 91 21 fd ff ff e3 00 2b ff d3 00 00 ec 25 ed ba 00 2c 2a d0 06 70 79 4c fa ff 00 11 00 b4 00 30 00 00 ec 25 ed ba 00 2c 2a cd 06 70 61 7b 24 ff ff a6 ff f9 00 83 00 00 ec 25 ed ba 00 2c 2a cd 06 70 49 aa 66 ff 00 7f ff 5c ff a4 00 00 ec 25 ed bd 00 25 2a cd 06 70 31 da d7 ff 00 6f 00 06 00 9f 00 00 ec 25 ed bd 0process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
0 1f 2a cd 06 70 1a 09 80 ff 00 94 00 5a 00 0f 00 00 ec 25 ed ba 00 1f 2a cd 06 70 02 37 7c ff 00 18 00 4c 00 19 00 00 ec 25 ed ba 00 22 2a cd 06 70 ea 65 15 ff 00 5e 00 3c 00 4e 00 00 ec 25 ed b7 00 22 2a c6 06 70 d2 8d 86 ff 00 68 00 64 00 a2 00 00 ec 25 ed b7 00 22 2a c6 06 70 ba bf 26 ff 00 1b 00 74 00 55 00 00 ec 25 ed b7 00 25 2a c6 06 70 a2 ef b7 ff 00 51 00 21 00 62 00 00 ec 25 ed b7 00 1f 2a cd 06 70 8b 1a bc ff 00 cd 00 5d 00 0f 00 00 ec 25 ed b3 00 1f 2a c6 06 70 73 43 27 ff ff ed 00 2e ff cc 00 00 ec 25 ed b7 00 22 2a d0 06 70 5b 70 f9 ff 00 4a ff f6 00 55 00 00 ec 25 ed ba 00 22 2a c6 06 70 43 9d b6 ff 00 22 00 1d ff ee 00 00 ec 25 ed bd 00 29 2a d3 06 70 2b cb 7b ff 00 58 00 ed 00 d8 00 00 ec 25 ed c4 00 25 2a d0 06 70 13 fb 84 ff 00 54 00 3c ff f5 00 00 ec 25 ed cb 00 25 2a d0 06 70 fc 26 06 ff 00 11 00 2b 00 0b 00 00 ec 25 ed c7 00 25 2a d0 06 70 e4 55 dc ff 00 7d 00 46 ff a5 00 00 ec 25 ed ce 00 26 2a ca 06 70 cc 84 15 ff ff cf 00 7e 00 1f 00 00 ec 25 ed c7 00 29 2a cd 06 70 b4 b5 31 ff 00 d4process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
 00 39 00 a9 00 00 ec 25 ed c7 00 29 2a cd 06 70 9c e1 90 ff 00 3d 00 50 ff ff 00 00 ec 25 ed b7 00 2c 2a c6 06 70 85 11 6a ff ff fd ff f6 ff eb 00 00 ec 25 ed b3 00 2c 2a c6 06 70 6d 3e cb ff ff e3 00 3b 00 55 00 00 ec 25 ed a9 00 2f 2a cd 06 70 55 6b 77 ff 00 65 00 93 00 2a 00 00 ec 25 ed a6 00 2c 2a d0 06 70 3d 98 39 ff 00 b2 ff e6 00 d1 00 00 ec 25 ed 9c 00 2c 2a d3 06 70 25 c6 8e ff 00 0d 00 00 00 5b 00 00 ec 25 ed 99 00 25 2a cd 06 70 0d f1 91 ff ff be 00 6e 00 65 00 00 ec 25 ed 9f 00 25 2a cd 06 70 f6 21 d8 ff ff a3 00 17 00 a8 00 00 ec 25 ed a3 00 25 2a cd 06 70 de 4e c2 ff 00 4b 01 1c 00 99 00 00 ec 25 ed a9 00 1f 2a cd 06 70 c6 7f 7b ff 00 98 00 75 00 00 00 00 ec 25 ed ad 00 1b 2a c9 06 70 ae ad 99 ff ff a3 00 4c ff f8 00 00 ec 25 ed b3 00 1f 2a c6 06 70 96 da 8d ff 00 72 ff d8 00 3a 00 00 ec 25 ed ba 00 1f 2a cd 06 70 7f 0a 52 ff 00 8e 00 aa ff a9 00 00 ec 25 ed c1 00 18 2a d0 06 70 67 39 c9 ff 00 6e ff ad 00 87 00 00 ec 25 ed c4 00 1f 2a d3 06 70 4f 66 ac ff ff 8f 00 2e 00 1f 00 00 ec 25 ed c4 00 process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
1f 2a d0 06 70 37 8f f4 ff ff fd ff db 00 51 00 00 ec 25 ed c1 00 25 2a d0 06 70 1f c1 5d ff 00 a1 00 2b 00 66 00 00 ec 25 ed bd 00 22 2a d0 06 70 07 eb 73 ff 00 4e 00 75 00 2a 00 00 ec 25 ed ba 00 2c 2a d0 06 70 f0 1b 4e ff ff c8 00 5d 00 4b 00 00 ec 25 ed b0 00 2f 2a cd 06 70 d8 4c df ff 00 6f 00 5a ff d7 00 00 ec 25 ed a9 00 2f 2a cd 06 70 c0 79 1d ff 00 5a ff b7 00 b6 00 00 ec 25 ed ad 00 2f 2a d0 06 70 a8 a9 63 ff 00 83 ff f7 00 6d 00 00 ec 25 ed b0 00 36 2a ca 06 70 90 d4 9a ff ff ef ff 63 00 65 00 00 ec 25 ed ad 00 33 2a d0 06 70 79 03 81 ff ff ce 00 03 00 36 00 00 ec 25 ed b7 00 2f 2a cd 06 70 61 32 ec ff 00 84 00 ae ff a9 00 00 ec 25 ed b7 00 2c 2a cd 06 70 49 62 d5 ff 00 9a 00 1e 00 7a 00 00 ec 25 ed c1 00 2c 2a d0 06 70 31 8e 4e ff ff d1 ff b6 ff bf 00 00 ec 25 ed c7 00 25 2a d0 06 70 19 be 76 ff 00 80 00 1e 00 a6 00 00 ec 25 ed c4 00 2c 2a cd 06 70 01 ea 8c ff ff f4 00 16 ff a8 00 00 ec 25 ed c7 00 2c 2a cd 06 70 ea 17 11 ff 00 39 00 0d 00 e5 00 00 ec 25 ed c4 00 25 2a d0 06 70 d2 44 9a ff 00 d3 fprocess_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
f e9 00 2a 00 00 ec 25 ed c7 00 25 2a cd 06 70 ba 71 69 ff 00 51 00 7f 00 81 00 00 ec 25 ed bd 00 25 2a c9 06 70 a2 9f dd ff ff cf 00 dc 00 6c 00 00 ec 25 ed ba 00 25 2a c9 06 70 8a cb b3 ff 00 6b 00 10 00 9b 00 00 ec 25 ed b3 00 29 2a c9 06 70 72 f8 c5 ff 00 07 00 50 00 29 00 00 ec 25 ed ba 00 29 2a d0 06 70 5b 2d 5b ff ff c8 00 93 00 cd 00 00 ec 25 ed b3 00 25 2a d7 06 70 43 56 0f ff ff d2 00 3f 00 47 00 00 ec 25 ed bd 00 1f 2a d7 06 70 2b 85 5a ff 00 40 00 14 00 30 00 00 ec 25 ed c1 00 22 2a d7 06 70 13 b3 a4 ff 00 33 01 30 00 63 00 00 ec 25 ed c4 00 1b 2a d0 06 70 fb e0 f1 ff 00 57 00 5a 00 69 00 00 ec 25 ed c7 00 18 2a cd 06 70 e4 09 53 ff 00 14 ff c4 00 26 00 00 ec 25 ed c4 00 1b 2a cd 06 70 cc 3c 51 ff ff ea 00 9d 00 15 00 00 ec 25 ed bd 00 1b 2a c6 06 70 b4 68 f5 ff 00 87 00 85 00 0f 00 00 ec 25 ed b7 00 18 2a c3 06 70 9c 97 80 ff 00 6c 00 93 00 16 00 00 ec 25 ed b0 00 18 2a c3 06 70 84 bf 83 ff ff d1 ff d5 00 c6 00 00 ec 25 ed a6 00 1f 2a cd 06 70 6c f2 fa ff 00 1f 00 bb 00 19 00 00 ec 25 ed 9f 00 1fprocess_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
process_packet: packet header mismatch
 2a cd 06 70 55 1e 91 ff ff fd ff ad 00 ab 00 00 ec 25 ed 9c 00 1f 2a cd 06 70 3d 48 00 ff ff e7 00 bb ff d3 00 00 ec 25 ed 9c 00 29 2a d0 06 70 25 76 43 ff 00 b5 ff f7 00 e9 00 00 ec 25 ed 9f 00 25 2a d0 06 70 0d a8 7d ff 01 09 ff ad 00 45 00 00 ec 25 ed 9c 00 22 2a cd 06 70 f5 d1 ec ff 00 04 00 67 00 0b 00 00 ec 25 ed a6 00 22 2a cd 06 70 de 02 8b ff ff 74 00 38 00 65 00 00 ec 25 ed ad 00 2c 2a cd 06 70 c6 32 4e ff ff e9 00 13 ff e4 00 00 ec 25 ed b7 00 2c 2a c9 06 70 ae 61 39 ff 00 83 00 0a 00 1c 00 00 ec 25 ed bd 00 2f 2a c6 06 70 96 8d 1e ff 00 d1 00 7c ff fc 00 00 ec 25 ed c1 00 33 2a cd 06 70 7e c0 e7 ff ff 6d 00 27 00 97 00 00 ec 25 ed cb 00 2f 2a d3 06 70 66 ed ea ff 00 8a 00 1e 00 98 00 00 ec 25 ed cb 00 2f 2a d0 06 70 4f 16 0f ff 00 2f 00 3c 00 15 00 00 ec 25 ed c7 00 2f 2a d3 06 70 37 41 61 ff 00 5b 00 0d 00 4b 00 00 ec 25 ed c4 00 2f 2a d0 06 70 1f 73 a8 ff 00 07 00 27 00 3d 00 00 ec 25 ed c1 00 36 2a cd 06 70 07 a3 79 ff ff e0 00 cf 00 2a 00 00 ec 25 ed ba 00 2f 2a cd 06 70 ef cd ea ff 00 6b 00

I'm a little worried, it seems to be a little bit difficult for me to understand this protocol :(

Actually, it seems that Hex values are OK even if the header check fails.. why this happens? :(
0
 

Author Comment

by:Marcus-Barnet
ID: 39797964
I updated the code with the checksum remainder routine, but it seems that it is not able to recognize any header.

Can it be because I used a typedef struct packet in place of the struct name for the process_packet(packet* p) function?

This is my current code:

#include <stdio.h>   /* Standard input/output definitions */
 #include <string.h>  /* String function definitions */
 #include <unistd.h>  /* UNIX standard function definitions */
 #include <fcntl.h>   /* File control definitions */
 #include <errno.h>   /* Error number definitions */
 #include <termios.h> /* POSIX terminal control definitions */
#include <netinet/in.h>

 /*
  * 'open_port()' - Open serial port 1.
  *
  * Returns the file descriptor on success or -1 on error.
  */

 int open_port(void)
 {
   int fd;                                   /* File descriptor for the port */

   fd = open("/dev/ttyUSB1", O_RDWR | O_NOCTTY | O_NDELAY);

   if (fd == -1)
   {                                              /* Could not open the port */
     fprintf(stderr, "open_port: Unable to open /dev/ttyS1 - %s\n",
             strerror(errno));
   }

   return (fd);
 }

typedef unsigned char BYTE;

#pragma pack(push, 1) // save current pack setting and set to 1
struct IMU_data_packet /* all shorts are network byte order! */
{
  BYTE header;
  unsigned short roll;
  unsigned short pitch;
  unsigned short yaw;
  unsigned short accX;
  unsigned short accY;
  unsigned short accZ;
  unsigned short temp;
  unsigned short time;
  BYTE chksum;
};
#pragma pack(pop)     // return to previous pack setting
 typedef struct IMU_data_packet packet;

void process_packet(packet* p) {

  BYTE* buf = (BYTE*) p + 1;
  unsigned int chksum = 0;

  if (p->header != 0xFF) {

     fprintf(stderr, "process_packet: packet header mismatch\n");

     return;
  }
  unsigned int i = 0;
  for (i = 0; i < 16; ++i) {  // calc. checksum

    chksum += buf[i];
  }

  if (chksum % 256 != p->chksum) {

     fprintf(stderr, "process_packet: packet checksum mismatch\n");

     return;
  }

  unsigned short roll = ntohs(p->roll);
  unsigned short pitch = ntohs(p->pitch);
  unsigned short yaw = ntohs(p->yaw);
  unsigned short accX = ntohs(p->accX);
  unsigned short accY = ntohs(p->accY);
  unsigned short accZ = ntohs(p->accZ);
  unsigned short temp = ntohs(p->temp);
  unsigned short time = ntohs(p->time);

  /* Now, do somthing with all that data! */
}
                                            
                                            

int main(void)
{
 int spfd=0;                                            /* File descriptor */
 unsigned char buf[18]; /* for one IMU data packet */
 struct termios options;
 
 spfd = open_port();

 fcntl(spfd, F_SETFL, FNDELAY);                  /* Configure port reading */
                                     /* Get the current options for the port */
 tcgetattr(spfd, &options);
 cfsetispeed(&options, B38400);                 /* Set the baud rates to 9600 */
 cfsetospeed(&options, B38400);
    
                                   /* Enable the receiver and set local mode */
 options.c_cflag |= (CLOCAL | CREAD);
 options.c_cflag &= ~PARENB; /* Mask the character size to 8 bits, no parity */
 options.c_cflag &= ~CSTOPB;
 options.c_cflag &= ~CSIZE;
 options.c_cflag |=  CS8;                              /* Select 8 data bits */
 options.c_cflag &= ~CRTSCTS;               /* Disable hardware flow control */  
 
                                 /* Enable data to be processed as raw input */
 options.c_lflag &= ~(ICANON | ECHO | ISIG);
       
                                        /* Set the new options for the port */
 tcsetattr(spfd, TCSANOW, &options);


unsigned char command='C';
write(spfd, &command, sizeof(command));  /* send G to switch to requested mode */
                      
 while (1)
 {   
    read(spfd, &buf, sizeof(buf));          /* Read packet */
    int i;
    for (i = 0; i < 22; i++) {
    printf("%02x ", buf[i]);
    }
   
   process_packet( (packet *) buf);

   usleep(2000);
 }
                                                    /* Close the serial port */
  close(spfd);

return 0;
 }
                                            

Open in new window

0
 

Author Comment

by:Marcus-Barnet
ID: 39797987
I added some print statements in the process_packet function in this way:

void process_packet(packet* p) {

  BYTE* buf = (BYTE*) p + 1;
  unsigned int chksum = 0;
  printf("HEADER: %d\n", p->header);
  if (p->header != 0xFF) {

     fprintf(stderr, "process_packet: packet header mismatch\n");

     return;
  }
  unsigned int i = 0;
  for (i = 0; i < 16; ++i) {  // calc. checksum
    printf("buf[%d]: %d\n", i, p->header);
    chksum += buf[i];
  }

  printf("CHECK: %d\n", chksum);

  if (chksum % 256 != p->chksum) {

     fprintf(stderr, "process_packet: packet checksum mismatch\n");

     return;
  }

  unsigned short roll = ntohs(p->roll);
  unsigned short pitch = ntohs(p->pitch);
  unsigned short yaw = ntohs(p->yaw);
  unsigned short accX = ntohs(p->accX);
  unsigned short accY = ntohs(p->accY);
  unsigned short accZ = ntohs(p->accZ);
  unsigned short temp = ntohs(p->temp);
  unsigned short time = ntohs(p->time);

  /* Now, do somthing with all that data! */
}

Open in new window


and I get this output:

HEADER: 0
process_packet: packet header mismatch
HEADER: 255
buf[0]: 255
buf[1]: 255
buf[2]: 255
buf[3]: 255
buf[4]: 255
buf[5]: 255
buf[6]: 255
buf[7]: 255
buf[8]: 255
buf[9]: 255
buf[10]: 255
buf[11]: 255
buf[12]: 255
buf[13]: 255
buf[14]: 255
buf[15]: 255
CHECK: 1603
process_packet: packet checksum mismatch
HEADER: 0
process_packet: packet header mismatch
HEADER: 0
process_packet: packet header mismatch
HEADER: 0
process_packet: packet header mismatch
HEADER: 0
process_packet: packet header mismatch
HEADER: 0
process_packet: packet header mismatch
HEADER: 255
buf[0]: 255
buf[1]: 255
buf[2]: 255
buf[3]: 255
buf[4]: 255
buf[5]: 255
buf[6]: 255
buf[7]: 255
buf[8]: 255
buf[9]: 255
buf[10]: 255
buf[11]: 255
buf[12]: 255
buf[13]: 255
buf[14]: 255
buf[15]: 255
CHECK: 1287
process_packet: packet checksum mismatch
HEADER: 0
process_packet: packet header mismatch
HEADER: 255
buf[0]: 255
buf[1]: 255
buf[2]: 255
buf[3]: 255
buf[4]: 255
buf[5]: 255
buf[6]: 255
buf[7]: 255
buf[8]: 255
buf[9]: 255
buf[10]: 255
buf[11]: 255
buf[12]: 255
buf[13]: 255
buf[14]: 255
buf[15]: 255
CHECK: 1545
process_packet: packet checksum mismatch
HEADER: 0
process_packet: packet header mismatch
HEADER: 0
process_packet: packet header mismatch
HEADER: 0
process_packet: packet header mismatch
HEADER: 255
buf[0]: 255
buf[1]: 255
buf[2]: 255
buf[3]: 255
buf[4]: 255
buf[5]: 255
buf[6]: 255
buf[7]: 255
buf[8]: 255
buf[9]: 255
buf[10]: 255
buf[11]: 255
buf[12]: 255
buf[13]: 255
buf[14]: 255
buf[15]: 255
CHECK: 1811

Can it really be a typedef problem?
If I use "void process_packet(IMU_data_packet* p)" gcc compiler gives me an error on IMU_data_packet not recognized so I have to typedef struct in order to avoid the error in this way:

struct IMU_data_packet /* all shorts are network byte order! */
{
  BYTE header;
  unsigned short roll;
  unsigned short pitch;
  unsigned short yaw;
  unsigned short accX;
  unsigned short accY;
  unsigned short accZ;
  unsigned short temp;
  unsigned short time;
  BYTE chksum;
};
#pragma pack(pop)     // return to previous pack setting

 typedef struct IMU_data_packet packet; //  <--- HERE

void process_packet(packet* p) {

  BYTE* buf = (BYTE*) p + 1;
  unsigned int chksum = 0;
  printf("HEADER: %d\n", p->header);
  if (p->header != 0xFF) {

     fprintf(stderr, "process_packet: packet header mismatch\n");

     return;
  }
..

}

Open in new window

0
 
LVL 86

Expert Comment

by:jkr
ID: 39798059
No, most unlikely to be a typedef preblem. But there also doesn't seem to be a '0xFF' as a correct header value either. Can you reset the device?
0
 

Author Comment

by:Marcus-Barnet
ID: 39798195
Do you mean trought the serial port or manually?

I can manually reset the device without problems.
Throught the serial port, I have to send "R"
0
 

Author Comment

by:Marcus-Barnet
ID: 39798233
I did the reset, but the result didn't changed so much.
Sometimes, I get 0xFF and the values change, but there is not match for the moment :(

This is the output:

mate@mate-core:~/Desktop/xbow$ gcc xbow1.c -o xbow2
mate@mate-core:~/Desktop/xbow$ ./xbow2
HEADER: 231
process_packet: packet header mismatch
HEADER: 230
process_packet: packet header mismatch
HEADER: 198
process_packet: packet header mismatch
HEADER: 215
process_packet: packet header mismatch
HEADER: 189
process_packet: packet header mismatch
HEADER: 181
process_packet: packet header mismatch
HEADER: 141
process_packet: packet header mismatch
HEADER: 123
process_packet: packet header mismatch
HEADER: 139
process_packet: packet header mismatch
HEADER: 119
process_packet: packet header mismatch
HEADER: 81
process_packet: packet header mismatch
HEADER: 98
process_packet: packet header mismatch
HEADER: 55
process_packet: packet header mismatch
HEADER: 57
process_packet: packet header mismatch
HEADER: 41
process_packet: packet header mismatch
HEADER: 20
process_packet: packet header mismatch
HEADER: 15
process_packet: packet header mismatch
HEADER: 231
process_packet: packet header mismatch
HEADER: 242
process_packet: packet header mismatch
HEADER: 217
process_packet: packet header mismatch
HEADER: 230
process_packet: packet header mismatch
HEADER: 197
process_packet: packet header mismatch
HEADER: 169
process_packet: packet header mismatch
HEADER: 165
process_packet: packet header mismatch
HEADER: 142
process_packet: packet header mismatch
HEADER: 128
process_packet: packet header mismatch
HEADER: 120
process_packet: packet header mismatch
HEADER: 110
process_packet: packet header mismatch
HEADER: 105
process_packet: packet header mismatch
HEADER: 67
process_packet: packet header mismatch
HEADER: 65
process_packet: packet header mismatch
HEADER: 61
process_packet: packet header mismatch
HEADER: 21
process_packet: packet header mismatch
HEADER: 61
process_packet: packet header mismatch
HEADER: 255
buf[0]: 255
buf[1]: 255
buf[2]: 255
buf[3]: 255
buf[4]: 255
buf[5]: 255
buf[6]: 255
buf[7]: 255
buf[8]: 255
buf[9]: 255
buf[10]: 255
buf[11]: 255
buf[12]: 255
buf[13]: 255
buf[14]: 255
buf[15]: 255
CHECK: 927
process_packet: packet checksum mismatch
HEADER: 253
process_packet: packet header mismatch
HEADER: 223
process_packet: packet header mismatch
HEADER: 227
process_packet: packet header mismatch
HEADER: 194
process_packet: packet header mismatch
HEADER: 217
process_packet: packet header mismatch
HEADER: 173
process_packet: packet header mismatch
HEADER: 141
process_packet: packet header mismatch
HEADER: 160
process_packet: packet header mismatch
HEADER: 126
process_packet: packet header mismatch
HEADER: 128
process_packet: packet header mismatch
HEADER: 117
process_packet: packet header mismatch
HEADER: 99
process_packet: packet header mismatch
HEADER: 71
process_packet: packet header mismatch
HEADER: 62
process_packet: packet header mismatch
HEADER: 44
process_packet: packet header mismatch
HEADER: 34
process_packet: packet header mismatch
HEADER: 37
process_packet: packet header mismatch
HEADER: 14
process_packet: packet header mismatch
HEADER: 238
process_packet: packet header mismatch
HEADER: 241
process_packet: packet header mismatch

and this is the current code:

#include <stdio.h>   /* Standard input/output definitions */
 #include <string.h>  /* String function definitions */
 #include <unistd.h>  /* UNIX standard function definitions */
 #include <fcntl.h>   /* File control definitions */
 #include <errno.h>   /* Error number definitions */
 #include <termios.h> /* POSIX terminal control definitions */
#include <netinet/in.h>

 /*
  * 'open_port()' - Open serial port 1.
  *
  * Returns the file descriptor on success or -1 on error.
  */

 int open_port(void)
 {
   int fd;                                   /* File descriptor for the port */

   fd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY);

   if (fd == -1)
   {                                              /* Could not open the port */
     fprintf(stderr, "open_port: Unable to open /dev/ttyS1 - %s\n",
             strerror(errno));
   }

   return (fd);
 }

typedef unsigned char BYTE;

#pragma pack(push, 1) // save current pack setting and set to 1
struct IMU_data_packet /* all shorts are network byte order! */
{
  BYTE header;
  unsigned short roll;
  unsigned short pitch;
  unsigned short yaw;
  unsigned short accX;
  unsigned short accY;
  unsigned short accZ;
  unsigned short temp;
  unsigned short time;
  BYTE chksum;
};
#pragma pack(pop)     // return to previous pack setting
 typedef struct IMU_data_packet packet;

void process_packet(packet* p) {

  BYTE* buf = (BYTE*) p + 1;
  unsigned int chksum = 0;
  printf("HEADER: %d\n", p->header);
  if (p->header != 0xFF) {

     fprintf(stderr, "process_packet: packet header mismatch\n");

     return;
  }
  unsigned int i = 0;
  for (i = 0; i < 16; ++i) {  // calc. checksum
    printf("buf[%d]: %d\n", i, p->header);
    chksum += buf[i];
  }

  printf("CHECK: %d\n", chksum);

  if (chksum % 256 != p->chksum) {

     fprintf(stderr, "process_packet: packet checksum mismatch\n");

     return;
  }

  unsigned short roll = ntohs(p->roll);
  unsigned short pitch = ntohs(p->pitch);
  unsigned short yaw = ntohs(p->yaw);
  unsigned short accX = ntohs(p->accX);
  unsigned short accY = ntohs(p->accY);
  unsigned short accZ = ntohs(p->accZ);
  unsigned short temp = ntohs(p->temp);
  unsigned short time = ntohs(p->time);

  /* Now, do somthing with all that data! */
}
                                            
                                            

int main(void)
{
 int spfd=0;                                            /* File descriptor */
 unsigned char buf[18]; /* for one IMU data packet */
 struct termios options;
 
 spfd = open_port();

 fcntl(spfd, F_SETFL, FNDELAY);                  /* Configure port reading */
                                     /* Get the current options for the port */
 tcgetattr(spfd, &options);
 cfsetispeed(&options, B38400);                 /* Set the baud rates to 9600 */
 cfsetospeed(&options, B38400);
    
                                   /* Enable the receiver and set local mode */
 options.c_cflag |= (CLOCAL | CREAD);
 options.c_cflag &= ~PARENB; /* Mask the character size to 8 bits, no parity */
 options.c_cflag &= ~CSTOPB;
 options.c_cflag &= ~CSIZE;
 options.c_cflag |=  CS8;                              /* Select 8 data bits */
 options.c_cflag &= ~CRTSCTS;               /* Disable hardware flow control */  
 
                                 /* Enable data to be processed as raw input */
 options.c_lflag &= ~(ICANON | ECHO | ISIG);
       
                                        /* Set the new options for the port */
 tcsetattr(spfd, TCSANOW, &options);


unsigned char command='C';
write(spfd, &command, sizeof(command));  /* send G to switch to requested mode */
                      
 while (1)
 {   
    read(spfd, &buf, sizeof(buf));          /* Read packet */
  
   
   process_packet( (packet *) buf);

   usleep(20000);
 }
                                                    /* Close the serial port */
  close(spfd);

return 0;
 }
                                            

Open in new window


I can't understand why we have this behavior since it seems that the algorithm is really OK now!
May be something wrong in the serial port configuration?
This is what it is specified in the manual:
The serial interface is standard RS-232, 38400 baud, 8 data bits, 1 start bit,
1 stop bit, no parity, and no flow control.

and it seems to be coincident with the code parameters.
0
 
LVL 86

Expert Comment

by:jkr
ID: 39798262
How exactly does the communication work? I mean after you send an 'R', is the next thing you receive an IMU packet? I.e. like

 write(spfd, "R", 1);
 while (1)
 {
   read(spfd, &buf, sizeof(buf));          /* Read packet */
   
   process_packet(IMU_data_packet*) buf);

   usleep(20000);
 }

Open in new window


?
0
 

Author Comment

by:Marcus-Barnet
ID: 39798563
When you send an R you just receive character H as reply: this means that IMU is working again and everything is OK.

I attached a table with all the commands.
The commands should be send as HEX and not as ASCII like the manual says:

*Note: argument of command is sent as a single hex byte, not as an ASCII
character.
imu-users-manual.pdf
command-table.png
0
 
LVL 86

Expert Comment

by:jkr
ID: 39798591
Yes, but when is the data sent? In order to skip the 'H', that could be

 write(spfd, "R", 1);
 read(spfd, buf, 1);
 while (1)
 {
   read(spfd, buf, sizeof(buf));          /* Read packet */
   
   process_packet(IMU_data_packet*) buf);

   usleep(20000);
 }

Open in new window

                                           
Oh, and look at the correction in the above,  the line

   read(spfd, &buf, sizeof(buf));          /* Read packet */

Open in new window


was incorrect and should have been

   read(spfd, buf, sizeof(buf));          /* Read packet */

Open in new window


Having a little hope that this actually was the culprit ;o)
0
 
LVL 32

Expert Comment

by:sarabande
ID: 39802697
buf and &buf have the same address for array variables. (this fact can be used to determine whether variables are pointer variables or array variables).

if the short members are network order (big endian), i wonder whether the single bytes for header and checksum may not needed to be switched as well with the corresponding odd or even neighbor byte. this depends on how the send buffer was built. if it was an array of shorts or if it was a packed structure equivalent to that posted in the original post.

only for the latter case the bytes for header and checksum could be taken directly from byte stream. if not, you first would need to fetch an array of shorts, then do the nhtos byte switch, and finally copy the results to a packed structure (there must not be any filler between the header byte and the first short member) like the one you posted.

Sara
0
 
LVL 32

Expert Comment

by:sarabande
ID: 39803092
that also would explain why the first FF is at byte 3 in the byte stream you posted and not  at an even byte position.

Sara
0
 

Author Comment

by:Marcus-Barnet
ID: 39803115
Hello Sara,

thank you for your suggestions! I've read the sensor manual again but I wasn't able to understand how the packet is sent from the sensor :(

With the JKR solution, the program seems to correctly receive packets from the IMU sensor but I'm still testing it because I'm not sure that everything is working well! :(

Are the information which you are talking about  in your last post present in the previous attached sensor manual?
0
 
LVL 32

Expert Comment

by:sarabande
ID: 39803390
Are the information which you are talking about  in your last post present in the previous attached sensor manual?
no, I was talking about the byte stream where the code you got from jkr assumed that it starts with the FF byte and then there are 7 short integers and finally there is a checksum byte.

if the byte stream was created from short array in network order each short integer needs to swap its bytes

for a stream that starts with

BYTE buf[] =  {  0x73, 0x00, 0x5a, 0xff, 0xfc, 0x00, 0x00, 0xec, 0x25, 0xed, 0xba, 0x00, ..

Open in new window


it would mean that any pair of bytes must be switched after reading the stream:


BYTE buf[] =  { 0x00,  0x73, 0xff, 0x5a, 0x00, 0xfc, 0xec, 0x00, 0xed, 0x25, 0x00, 0xba, ..

Open in new window

if that is correct it would have a crucial importance for detecting the header byte, for the bytes that would be used for calculating the checksum and for the checksum byte.

to test if that could be the issue you could make the swap on the buf elements before passing them to process_packet function. however, that might create some further issues which also must be considered. for example, if the byte stream you posted is real, the first packet would not start with FF but with 00. this is either an error or the packet starts with an offset of 2. i would assume it is the latter. then, the end of a packet is either determined by the fixed length of 16 bytes (as assumed by the current code) or it is determined by (any of the) next FF where a new packet starts. for the stream you posted the 0x73 proceeding the 1st FF could be a code to identify the packet structure (it also could be a length or size or the count of packages available). anyway, if we assume that we have packets of different lengths perhaps prefixed by some type information, it is a new game.

Sara
0
 
LVL 32

Expert Comment

by:sarabande
ID: 39804332
input='FFFFB30029FF86FF8A0009FFD5FFB22AC20632A48CCB';
when I use the matlab sample data for verifying the checksum it is ok.

but the sequence has a length of 22 bytes while the length of the packet structure is 18 bytes.

obviously only one of both could be describe a correct sequence.

the matlab calculation is somewhat strange what perhaps is due to it cannot extract bytes or short integers but uses strings and numbers instead.

input=input(3:end);
s=length(input)-2;
n=s/4;
for i=1:n
    a(i,1:4)=input(4*(i-1)+1:4*i);
end

for i=1:n
    str=a(i,:);
if hex2dec(str(1))>=8
   o1(i,1)=hex2dec(str)-2^16;
else
   o1(i,1)=hex2dec(str);
end

Open in new window


the code extracts the header FF from input string, and calculates the number of 4-byte elements (what is 5 for our case). it stores the 4-byte strings in array a. after that it does a loop on array a and converts each 4-byte string with hex2dec function. despite of the name 'dec' i think it converts to integer (binary) such that the 20 bytes were reduced to 10 bytes or 5 short integers. note, because a string number has the most significant digit at the left side - same as for big-endian or network order - matlab implicitly makes a nthos swap of the bytes. note a further detail: if the short number has the sign-bit set (high-byte >= '8') the number was made negative by substracting 2^16 from it. these numbers were stored in the o1 array (also 5 elements).

o1sc(1:2,1)=o1(1:2,1)*sfan; % (deg) Roll and Pitch 
o1sc(3:5,1)=o1(3:5,1)*sfr; %deg/s Tilt rate
o1sc(6:8,1)=o1(6:8,1)*sfa; % g accelerations
o1sc(9,1)=((o1(9,1) * 5/4096)-1.375)*44.44; % Temperature (8C)
 x = int16(o1(10,1));
  if x < 0
    y = uint16(double(x) + 65536);
  else
  y = uint16(x);
  end
o1sc(10,1)=double(y)*790*10^-9; 
o1sc

Open in new window


- the first two bytes (== 1st element of o1) were used to calculate sfan.

I wonder how matlab could store the result of short * double  in 2 characters of o1sc string array but anyway.

- the next 3 bytes (!sic) were used to calc the Tilt rate.
- again 3 bytes for acceleration
- byte 9 for temperature
- byte 10 has a strange calculation which finally seems to be stored in one byte.

I can't help you with this but you could try to find out how o1 and o1sc and the functions hex2dec and int16/uint16 were defined. perhaps it makes things clearer.

anyway the calculation and the info you posted with the table.png have little in common. here you need to find a better specification.

Sara
0
 

Author Comment

by:Marcus-Barnet
ID: 39806272
The Matlab code was made by my friend who tried to make the sensor work with Windows and Matlab and so it might be not correct even if the output is surely correct because we did a compare between matlab output and the output of thw Windows sensor software and they are the same.

For this reason, I was thinking to translate the matlab code in C code with the same algorithm even if in Matlab each received packet is 22 bytes even if in the manual there is specified that each packet should be 18 byte lenght.

The hex2dec function is described as: http://www.mathworks.it/it/help/matlab/ref/hex2dec.html
0
 
LVL 32

Expert Comment

by:sarabande
ID: 39806334
if using the algorithm of your friend you would need the 20 half-bytes which represent 5 short numbers.

the packet structure holds 8 short integers, so actually it is more than used by matlab.

nevertheless there is a mismatch between the stream you read from the device and the one used in the matlab. also from your description it seems to be that each read would return a full packet starting with a FF and a checksum byte at end. that also doesn't fit to the data you got.

if you would post all bytes of the stream in hex (without other messages) we could try to find out whether there are some systematics. at least we should be able to find the begin and end of sequences where the described checksum algorithm would apply to.

Sara
0
 
LVL 86

Expert Comment

by:jkr
ID: 39808188
Well, what are te issues you are facing? Given the specs from the abov, the code should work well...
0
 

Author Comment

by:Marcus-Barnet
ID: 39812474
Hello Sarabande and JKR,

i'm sorry for the delay, I had problems with my internet connection.

I'm doing tests to verify if the code works well..
In the meanwhile, I modified the code in order to print HEX values as requested and this is the output (sometimes, it don't recognize the checksum but I think this isn't a problem):

user@mate-core:~/Desktop/xbow$ ./imu
FF080B080800080307FE04C105FEEA5E5400698A0408
process_packet: packet checksum mismatch
FF080308000806080007FE04B005FF5D2063698A0408
FF07F908090802080407FD04C105FFE3340B698A0408
FF080C08150809080107FE04C005FF6E1AA0698A0408
FF07FD080A07FB080207FE04C105FFF42108698A0408
process_packet: packet checksum mismatch
FF080A0807080A080307FE04C105FF83F382698A0408
FF07FA08070808080307FE04C105FF0ED7DE698A0408
FF0806080A080B080307FE04C105FF99B75C698A0408
FF081408040805080307FE04C105FF1FC4F1698A0408
FF07FE080607FF080307FE04C105FFAF8425698A0408
FF08000805080B080307FE04C105FF3A77B2698A0408
FF07FF08060802080307FE04C105FFC5631F698A0408
FF07FF07FB0816080307FE04C105FF4B72BC698A0408
FF080A08100807080307FE04C105FFDB3727698A0408
process_packet: packet checksum mismatch
FF07FB080508170804080004C205FF662799698A0408
FF07F808080F080307FD04C10600EC342999698A0408
process_packet: packet checksum mismatch

and this is the C code I'm using at the moment:

#include <stdio.h>   /* Standard input/output definitions */
 #include <string.h>  /* String function definitions */
 #include <unistd.h>  /* UNIX standard function definitions */
 #include <fcntl.h>   /* File control definitions */
 #include <errno.h>   /* Error number definitions */
 #include <termios.h> /* POSIX terminal control definitions */
#include <netinet/in.h>

 /*
  * 'open_port()' - Open serial port 1.
  *
  * Returns the file descriptor on success or -1 on error.
  */

 int open_port(void)
 {
   int fd;                                   /* File descriptor for the port */

   fd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY);

   if (fd == -1)
   {                                              /* Could not open the port */
     fprintf(stderr, "open_port: Unable to open /dev/ttyS1 - %s\n",
             strerror(errno));
   }

   return (fd);
 }

typedef unsigned char BYTE;

#pragma pack(push, 1) // save current pack setting and set to 1
struct IMU_data_packet /* all shorts are network byte order! */
{
  BYTE header;
  unsigned short roll;
  unsigned short pitch;
  unsigned short yaw;
  unsigned short accX;
  unsigned short accY;
  unsigned short accZ;
  unsigned short temp;
  unsigned short time;
  BYTE chksum;
};
#pragma pack(pop)     // return to previous pack setting
 typedef struct IMU_data_packet packet;

void process_packet(packet* p) {

  BYTE* buf = (BYTE*) p + 1;
  unsigned int chksum = 0;

  if (p->header != 0xFF) {

     fprintf(stderr, "process_packet: packet header mismatch\n");

     return;
  }
  unsigned int i = 0;
  for (i = 0; i < 16; ++i) {  // calc. checksum
    
    chksum += buf[i];
  }

 // printf("CHECK: %d\n", chksum);

  if (chksum % 256 != p->chksum) {

     fprintf(stderr, "process_packet: packet checksum mismatch\n");

     return;
  }
  
  unsigned short roll = ntohs(p->roll);
 // printf("Roll: %f", roll*0.005);
  unsigned short pitch = ntohs(p->pitch);
  unsigned short yaw = ntohs(p->yaw);
  unsigned short accX = ntohs(p->accX);
  unsigned short accY = ntohs(p->accY);
  unsigned short accZ = ntohs(p->accZ);
  unsigned short temp = ntohs(p->temp);
  unsigned short time = ntohs(p->time);

  /* Now, do somthing with all that data! */
}
                                            
                                            

int main(void)
{
 int spfd=0;                                            /* File descriptor */
 unsigned char buf[22]; /* for one IMU data packet */
 struct termios options;
 
 spfd = open_port();

 fcntl(spfd, F_SETFL, FNDELAY);                  /* Configure port reading */
                                     /* Get the current options for the port */
 tcgetattr(spfd, &options);
 cfsetispeed(&options, B38400);                 /* Set the baud rates to 9600 */
 cfsetospeed(&options, B38400);
    
                                   /* Enable the receiver and set local mode */
 options.c_cflag |= (CLOCAL | CREAD);
 options.c_cflag &= ~PARENB; /* Mask the character size to 8 bits, no parity */
 options.c_cflag &= ~CSTOPB;
 options.c_cflag &= ~CSIZE;
 options.c_cflag |=  CS8;                              /* Select 8 data bits */
 options.c_cflag &= ~CRTSCTS;               /* Disable hardware flow control */  
 
                                 /* Enable data to be processed as raw input */
 options.c_lflag &= ~(ICANON | ECHO | ISIG);
       
                                        /* Set the new options for the port */
 tcsetattr(spfd, TCSANOW, &options);



// write(spfd, "R", 1);
// read(spfd, buf, 1); 
char converted[22*2 + 1];                     
 while (1)
 {  
int i = 0; 
    write(spfd, "G", 1);
    read(spfd, buf, sizeof(buf));          /* Read packet */
   
     for(i=0;i<22;i++) {
    sprintf(&converted[i*2], "%02X", buf[i]);
  }
  printf("%s\n", converted);
   process_packet( (packet *) buf);

   usleep(2000000);
 }
                                                    /* Close the serial port */
  close(spfd);

return 0;
 }
                                            

Open in new window

imu-users-manual.pdf
0
 
LVL 86

Expert Comment

by:jkr
ID: 39812488
Well, since there is a chance that sometimes the checksum does not match, just discard these packets and move on processing others... that's at least what the checksum is for ;o)
0
 

Author Comment

by:Marcus-Barnet
ID: 39812769
I think there is something wrong with the code interpretation since IMU should send also some negative values while all I receive are only positive values.
In addition, the values seem to not change according on how I move the sensor.. mmh.. :(
0
 
LVL 86

Expert Comment

by:jkr
ID: 39812810
Wait a minute - how is that data to be interpreted? When you use

printf("Roll: %f", roll*0.005);

Open in new window


on an 'unsigned short', you will get '0'. Are these supposed to be floating-point values?
0
 

Author Comment

by:Marcus-Barnet
ID: 39812826
yes, I suppose to read floating point values..

At the moment, the output is:


Roll: 11.271973 Pitch: 11.315918 Tilt: 9.411621 AccX: 0.188599 AccY: 0.187408 AccZ: 0.111328

Roll: 11.288452 Pitch: 11.332397 Tilt: 9.420776 AccX: 0.188599 AccY: 0.187408 AccZ: 0.111053

Roll: 11.266479 Pitch: 11.315918 Tilt: 9.397888 AccX: 0.188599 AccY: 0.187500 AccZ: 0.111328

Roll: 11.293945 Pitch: 11.239014 Tilt: 9.420776 AccX: 0.188873 AccY: 0.187408 AccZ: 0.111420

Roll: 11.304932 Pitch: 11.387329 Tilt: 9.420776 AccX: 0.188965 AccY: 0.187317 AccZ: 0.110962

Roll: 11.310425 Pitch: 11.282959 Tilt: 9.407043 AccX: 0.188416 AccY: 0.187225 AccZ: 0.111237

Roll: 11.293945 Pitch: 11.299438 Tilt: 9.370422 AccX: 0.188965 AccY: 0.187500 AccZ: 0.111328

Roll: 11.288452 Pitch: 11.260986 Tilt: 9.411621 AccX: 0.188599 AccY: 0.187317 AccZ: 0.111328
0
How your wiki can always stay up-to-date

Quip doubles as a “living” wiki and a project management tool that evolves with your organization. As you finish projects in Quip, the work remains, easily accessible to all team members, new and old.
- Increase transparency
- Onboard new hires faster
- Access from mobile/offline

 
LVL 86

Expert Comment

by:jkr
ID: 39812857
Well, then th eonly remaining problem is interpreting the data correctly. Do the docs say anything about the format the data is stored in?

BTW, 'ntohs()' won't make much sense in this case...
0
 

Author Comment

by:Marcus-Barnet
ID: 39812913
Yes, the manual describes the output format in this way:

The IMU300CC, IMU400CC and IMU400CD are designed to operate as
six-axis systems and can be set to operate in one of two modes: voltage
mode, or scaled sensor mode. The IMU Series of products do not support
angle mode. The measurement mode selects the information that is sent in
the data packet over the RS-232 interface. See “Data Packet Format” for
the actual structure of the data packet in each mode.

On page 9 and page 10 (and 16,17) there are some specifications about voltage mode and scaled mode.
The main problem is that to set the sensor in each mode you have to send a command before.
For example, you have to send "c" for scaled mode and "r" for voltage mode.
Then the sensor will reply with a "C" or "R" if everything was OK.

I'm not able to get the correct reply from the sensor and so may be I'm not able to establish if the sensor is in voltage or scaled mode.
In any case, I think that the current output values are not correct because i do not get any negative value and the values don't change according on how I move it.
imu-users-manual.pdf
0
 
LVL 32

Expert Comment

by:sarabande
ID: 39812938
i run a little program which tests the checksum on the data you posted:

checksum mismatch: 97 <>  0  length of sequence[  0: 17] is  18
valid checksum check 63 for sequence[ 22: 39] , length is  18
valid checksum check  b for sequence[ 44: 61] , length is  18
valid checksum check a0 for sequence[ 66: 83] , length is  18
checksum mismatch:  5 <>  8  length of sequence[ 88:105] is  18
valid checksum check 82 for sequence[110:127] , length is  18
valid checksum check de for sequence[132:149] , length is  18
valid checksum check 5c for sequence[154:171] , length is  18
valid checksum check f1 for sequence[176:193] , length is  18
valid checksum check 25 for sequence[198:215] , length is  18
valid checksum check b2 for sequence[220:237] , length is  18
valid checksum check 1f for sequence[242:259] , length is  18
valid checksum check bc for sequence[264:281] , length is  18
checksum mismatch: 24 <> 27  length of sequence[286:303] is  18
valid checksum check 99 for sequence[308:325] , length is  18
checksum mismatch: 41 <> 99  length of sequence[330:347] is  18

Open in new window


it confirms the results you got. note, the data were read in chunks of 22 bytes, while the sequence from header-byte to checksum-byte is only 18. you easily see that the last byte of the 22-byte sequence is always 0x08 and therefore is not a checksum. while the 18-th byte of each 22-byte sequence is a valid checksum for 12 of 16 reads what is significant that the checksum calculation is correct though i don't have an idea why it is wrong for 4 records.

the last 4 bytes of each 22-byte sequence are constant with value 698A0408.

numbers containing hex digits are always positive. but any pair between header-byte and checksum where the 2nd byte begins with '8' or higher, actually is a negative short integer.

for example  07F9 in the 3rd row is 0xF907 when converted to little-endian what is -1785  when converted to decimal.

In addition, the values seem to not change according on how I move the sensor.. mmh.. :(


that could be a indication that the device is not sending sensor data but fixed data like configuration data.

Sara
0
 
LVL 86

Expert Comment

by:jkr
ID: 39812942
Yes, got that. But since IEEE defines 'float' as four-byte values (sign bit, 8-bit exponent, 23-bit mantissa), I wonder if there is any hint on how to interpret the two-byte fields in the packet...
0
 

Author Comment

by:Marcus-Barnet
ID: 39812963
Yes, manual says this:

In general, the digital data representing each measurement is sent as a 16-bit
number (two bytes). The data is sent MSB first then LSB.
In voltage mode, the data is sent as unsigned integers to represent the range 0 – 5 V.
In scaled and angle mode, the data generally represents a quantity that can be positive or negative.
These numbers are sent as a 16-bit signed integer in 2's complement format. The data is sent as two bytes, MSB first then LSB.
In scaled and angle mode, the timer information and temperature sensor
voltage are sent as unsigned integers.

What's the best solution to print also negative values?

for example  07F9 in the 3rd row is 0xF907 when converted to little-endian what is -1785  when converted to decimal.

Do I have to verify if values begins with "8"?
0
 
LVL 86

Expert Comment

by:jkr
ID: 39813011
Ahhhh, OK - well, then remove the 'unsigned' from the structures declaration, i.e.

pragma pack(push, 1) // save current pack setting and set to 1
typedef struct IMU_data_packet /* all shorts are network byte order! */
{
  BYTE header;
  short roll;
  short pitch;
  short yaw;
  short accX;
  short accY;
  short accZ;
  short temp;
  short time;
  BYTE chksum;
};
#pragma pack(pop)     // return to previous pack setting

Open in new window


and

  short roll = ntohs(p->roll);
  short pitch = ntohs(p->pitch);
  short yaw = ntohs(p->yaw);
  short accX = ntohs(p->accX);
  short accY = ntohs(p->accY);
  short accZ = ntohs(p->accZ);
  short temp = ntohs(p->temp);
  short time = ntohs(p->time);

Open in new window


Then, you have to 'scale' the values, e.g.

  short roll = ntohs(p->roll);
// ...


printf("Roll: %f", (float) roll*0.005);

Open in new window

0
 

Author Comment

by:Marcus-Barnet
ID: 39813065
Thank you JKR for your help!

I did the changes as you suggested: it seems that output is a little bit better now.. but I still can't get negative values and for this reason output values are still not correct :(

Do I have to use any particular algorithm to represent negative values?

Thank you again!

#include <stdio.h>   /* Standard input/output definitions */
 #include <string.h>  /* String function definitions */
 #include <unistd.h>  /* UNIX standard function definitions */
 #include <fcntl.h>   /* File control definitions */
 #include <errno.h>   /* Error number definitions */
 #include <termios.h> /* POSIX terminal control definitions */
#include <netinet/in.h>
#include <math.h>

 /*
  * 'open_port()' - Open serial port 1.
  *
  * Returns the file descriptor on success or -1 on error.
  */

 int open_port(void)
 {
   int fd;                                   /* File descriptor for the port */

   fd = open("/dev/ttyUSB1", O_RDWR | O_NOCTTY | O_NDELAY);

   if (fd == -1)
   {                                              /* Could not open the port */
     fprintf(stderr, "open_port: Unable to open /dev/ttyS1 - %s\n",
             strerror(errno));
   }

   return (fd);
 }

typedef unsigned char BYTE;

#pragma pack(push, 1) // save current pack setting and set to 1
struct IMU_data_packet /* all shorts are network byte order! */
{
  BYTE header;
   short roll;
   short pitch;
   short yaw;
   short accX;
   short accY;
   short accZ;
   short temp;
   short time;
  BYTE chksum;
};
#pragma pack(pop)     // return to previous pack setting
 typedef struct IMU_data_packet packet;

void process_packet(packet* p) {

  BYTE* buf = (BYTE*) p + 1;
  unsigned int chksum = 0;

  if (p->header != 0xFF) {

     fprintf(stderr, "process_packet: packet header mismatch\n");

     return;
  }
  unsigned int i = 0;
  for (i = 0; i < 16; ++i) {  // calc. checksum
    
    chksum += buf[i];
  }

 // printf("CHECK: %d\n", chksum);

  if (chksum % 256 != p->chksum) {

     fprintf(stderr, "process_packet: packet checksum mismatch\n");

     return;
  }
  double potenza = pow(2, 15);
   short roll = ntohs(p->roll);
  printf("\n %f ", (float)((roll*180)/potenza));
   short pitch = ntohs(p->pitch);
  printf(" %f ", (float)((pitch*180)/potenza));
   short yaw = ntohs(p->yaw);
  printf(" %f ", (float)(yaw*100*1.5/potenza));
   short accX = ntohs(p->accX);
printf(" %f ", (float)(accX*2*1.5/potenza));
   short accY = ntohs(p->accY);
printf(" %f ", (float)(accY*2*1.5/potenza));
   short accZ = ntohs(p->accZ);
printf(" %f \n", (float)(accZ*2*1.5/potenza));
   short temp = ntohs(p->temp);
   short time = ntohs(p->time);

  /* Now, do somthing with all that data! */
}
                                            
                                            

int main(void)
{
 int spfd=0;                                            /* File descriptor */
 unsigned char buf[22]; /* for one IMU data packet */
 struct termios options;
 
 spfd = open_port();

 fcntl(spfd, F_SETFL, FNDELAY);                  /* Configure port reading */
                                     /* Get the current options for the port */
 tcgetattr(spfd, &options);
 cfsetispeed(&options, B38400);                 /* Set the baud rates to 9600 */
 cfsetospeed(&options, B38400);
    
                                   /* Enable the receiver and set local mode */
 options.c_cflag |= (CLOCAL | CREAD);
 options.c_cflag &= ~PARENB; /* Mask the character size to 8 bits, no parity */
 options.c_cflag &= ~CSTOPB;
 options.c_cflag &= ~CSIZE;
 options.c_cflag |=  CS8;                              /* Select 8 data bits */
 options.c_cflag &= ~CRTSCTS;               /* Disable hardware flow control */  
 
                                 /* Enable data to be processed as raw input */
 options.c_lflag &= ~(ICANON | ECHO | ISIG);
       
                                        /* Set the new options for the port */
 tcsetattr(spfd, TCSANOW, &options);



// write(spfd, "R", 1);
// read(spfd, buf, 1); 
char converted[22*2 + 1];                     
 while (1)
 {  
int i = 0; 
    write(spfd, "G", 1);
    read(spfd, buf, sizeof(buf));          /* Read packet */
   
     for(i=0;i<22;i++) {
  //  sprintf(&converted[i*2], "%02X", buf[i]);
  }
 //  printf("%s\n", converted);
   process_packet( (packet *) buf);

   usleep(2000000);
 }
                                                    /* Close the serial port */
  close(spfd);

return 0;
 }
                                            

Open in new window

0
 
LVL 86

Expert Comment

by:jkr
ID: 39813085
Hmm, isn't 2^15 a little big to divide something with? The result will almost certainly be close to 0.

Which mode are you operating the sensor in?
0
 

Author Comment

by:Marcus-Barnet
ID: 39813272
Previous output was for Voltage mode while this output is for scaled mode:

Roll -0.027466  Pitch 0.109863  Yaw -0.169373  AccX 0.020508  AccY0.002289  AccZ 1.002502

 Roll 0.060425  Pitch 0.170288  Yaw 0.439453  AccX 0.018951  AccY-0.000366  AccZ 1.017334

 Roll -0.225220  Pitch 1.928101  Yaw 44.641113  AccX 0.016663  AccY0.026642  AccZ 1.014404

 Roll 0.225220  Pitch 0.335083  Yaw -0.242615  AccX 0.020782  AccY0.007141  AccZ 1.002777
process_packet: packet checksum mismatch

 Roll 0.351562  Pitch -0.208740  Yaw 0.869751  AccX 0.017120  AccY-0.002472  AccZ 1.001862

 Roll 0.983276  Pitch 0.131836  Yaw 0.379944  AccX 0.018951  AccY0.007782  AccZ 1.001587

With scaled mode I can have negative values in output and this is more correct than voltage mode.
0
 
LVL 86

Expert Comment

by:jkr
ID: 39813298
Now, if we could verify that they're accurate, we'd be done ;o)
0
 

Author Comment

by:Marcus-Barnet
ID: 39817184
Hello,

yesterday I tested the code in my lab by using also the windows official software for my old sensor.

I realized that manual specifications about the protocol are not correct and this is the correct procedure to decode the packet from the serial port:

After sending the command "G" to the serial port, the sensor answer with a full packet which is stored in "buf".
The content of the package is this:
FF001EFF380076FFABFFFB00DC002B2AF1066A718DFF

Where:
FF is the header for a correct packet
001E is the value for the ROLL
FF38 is the value for the PITCH
0076 is the value for the ROLL rate
FFAB is the PITCH rate
FFFB is the YAW rate
00DC is the Acc X
002B is the Acc Y
2AF1 is the Acc Z
066A is the Temperature
718D is the sample time
FF is the checksum

So, after receiving this packet, I think I should split it and convert all that values in decimal values and then apply a scale factor in order to get the correct floating point value.

For example, to get the ROLL for this packet, I should convert from HEX to DEC the value 001E which is 30 and then apply the scale factor which is:
30 * 180 / 2^15 = 0.16 degrees

I do not know if it is correct, but I tried to write this code to split the received packet in "buf" in this way:

char converted[22*2 + 1]; 
char stringa[44] = {0};
char stringa2[44] = {0};
char stringa3[2];
char car1;
char car2;
      int x = 0;    

char header[30];
char roll[30];
char pitch[30];
char roll_rate[30];
char pitch_rate[30];
char yaw_rate[30];  
char accx[30];
char accy[30];
char accz[30];
char temp[30];
char checksum[30];
char time[30];       
 while (1)
 {  
int i = 0; 
    write(spfd, "G", 1); // send G to ask for a full packet
    read(spfd, buf, sizeof(buf));          /* Read packet */
  # if 1
     for(i=0;i<22;i++) {
    sprintf(&converted[i*2], "%02X", buf[i]); // I convert in HEX values
  }
   
 printf("\n##########\n");
 
 printf("Full HEX packet: %s\n", converted);

char *c1 = substr(converted,0,2);
char *c2 = substr(converted,2,6);
char *c3 = substr(converted,6,10);
char *c4 = substr(converted,10,14);
char *c5 = substr(converted,14,18);
char *c6 = substr(converted,18,22);
char *c7 = substr(converted,22,26);
char *c8 = substr(converted,26,30);
char *c9 = substr(converted,30,34);
char *c10 = substr(converted,34,38);
char *c11 = substr(converted,38,42);
char *c12 = substr(converted,42,44);



strcpy(header, c1);
strcpy(roll, c2);
strcpy(pitch, c3);
strcpy(roll_rate, c4);
strcpy(pitch_rate, c5);
strcpy(yaw_rate, c6);
strcpy(accx, c7);
strcpy(accy, c8);
strcpy(accz, c9);
strcpy(temp, c10);
strcpy(time, c11);
strcpy(checksum, c12);

printf("header => %s, roll => %s, pitch => %s, roll_rate => %s, pitch_rate => %s, yaw_rate => %s, accx => %s, accy => %s, accz => %s, temp => %s, time = %s, checksum = %s", header, roll, pitch, roll_rate, pitch_rate, yaw_rate, accx, accy, accz, temp, time, checksum);

Open in new window


Now, if it is correct, I should convert roll, pitch, roll_rate and all the other variables in decimal values and apply the scaling factor for each one but I do not know how to do this in the correct way.. can you help me, please?
0
 
LVL 32

Expert Comment

by:sarabande
ID: 39817504
looks great.

for the sequence you posted I get correct checksum:

valid checksum check ff for sequence[  0: 21] , length is  22

Open in new window


you could do the check by

unsigned int cs = 0;
for (int n = 1; n < 21; ++n)
{
      cs += (unsigned char)buf[n];
}
cs %= 0x100;   // modulo 256 makes a cast to BYTE 
if (cs != (unsigned int)buf[21])
{
        // wrong checksum

Open in new window


for building the short integers it is not necessary to convert to string, then build strings and finally convert again to short (even if that also does the big-endian to little-endian). instead you could do like

short bytesToShort(BYTE * buf, size_t szBuf, int n)
{
    short s = 0;
    if (n >= 0 && n < (int)(szBuf-1))
    {
        unsigned short x = buf[n];   // assign first byte as low byte to short
        x <<= 8;                                  // shift low byte to high byte (8-bit shift to the left)
        x += buf[n+1];                       // 'add'  second byte as low byte  

        s = (short)x;
    }
    return s;
}

short sroll = bytesToShort(buf, sizeof(buf), 1); 
short spitch = bytesToShort(buf, sizeof(buf), 3); 
....

Open in new window


for the floating point calculations you should use separate float or double variables.

double droll = ((double)sroll * 180) / 32768; 

Open in new window


the 32768 is 2^15 but it is a little bit strange where I am not sure that the division is correct or only happens because some lacks of matlab to handle floating point numbers correctly.

Sara
0
 
LVL 86

Expert Comment

by:jkr
ID: 39817936
Wait now that is sent as a string instead of a binary value?
0
 

Author Comment

by:Marcus-Barnet
ID: 39818903
It's always a binary value but I prefer to look at it as a HEX string since it is more clear for me.. mmh..
0
 
LVL 86

Expert Comment

by:jkr
ID: 39818930
Well, that's a fundamental difference ;o)

By using a struct as above, the binary values will automately be separated into their respective fields, like using a stencil or template. That's a lot more convenient than parsing a hex string.
0
 

Author Comment

by:Marcus-Barnet
ID: 39819366
Hello jkr, do you mean this struct?

struct IMU_data_packet /* all shorts are network byte order! */
{
  BYTE header;
   short roll;
   short pitch;
   short yaw;
   short accX;
   short accY;
   short accZ;
   short temp;
   short time;
  BYTE chksum;
};

Open in new window



I tried the sarabande solution and I verified that all values are OK and also the checksum is OK.

Do you think that the struct usage in place of the hex string parsing may improve the code?
0
 
LVL 86

Expert Comment

by:jkr
ID: 39819388
>>Do you think that the struct usage in place of the hex string parsing may improve the code?

Let's look at it like this: First, you convert binary data to a string. Then you parse that string. Then you convert all that back to binary (the values you are using later on are binary after all). Isn't that quite a detour?
0
 

Author Comment

by:Marcus-Barnet
ID: 39819449
Yes, it is for sure!!
But I can't figure out how to avoid to parse the string and directly use that binary data to display decimal values following the algorithm which I described before. :(
0
 
LVL 86

Expert Comment

by:jkr
ID: 39819520
As long as you feel uncomfortable about fowarding a picture you received by mail by printing it and then scanning it again, I'd keep trying ;o)

But: The way you described the data as


FF001EFF380076FFABFFFB00DC002B2AF1066A718DFF

Where:
FF is the header for a correct packet
001E is the value for the ROLL
FF38 is the value for the PITCH
0076 is the value for the ROLL rate
FFAB is the PITCH rate
FFFB is the YAW rate
00DC is the Acc X
002B is the Acc Y
2AF1 is the Acc Z
066A is the Temperature
718D is the sample time
FF is the checksum

the "rate" values were comepletely missing in the description, which for sure would have caused that problem, sinc ethat should have been

#pragma pack(push, 1) // save current pack setting and set to 1
typedef struct IMU_data_packet /* all shorts are network byte order! */
{
  BYTE header;
  short roll;
  short pitch;
  short roll_rate;
  short pitch_rate;
  short yaw_rate;
  short accX;
  short accY;
  short accZ;
  short temp;
  short time;
  BYTE chksum;
};

Open in new window

0
 

Author Comment

by:Marcus-Barnet
ID: 39819530
Ops, sorry, i'm not english speaker.. what do you mean with:

As long as you feel uncomfortable about fowarding a picture you received by mail by printing it and then scanning it again, I'd keep trying ;o)

? :) :)
0
 
LVL 86

Expert Comment

by:jkr
ID: 39819542
Well, me neither - it basically refers to doing something in a long-wound way instead of taking the straightfoward route.

But anyway, the missing to "rate" fields (check the initial picture that you posted) would explain the incorrect results.
0
 
LVL 32

Accepted Solution

by:
sarabande earned 250 total points
ID: 39820459
I tried the sarabande solution and I verified that all values are OK and also the checksum is OK.

Do you think that the struct usage in place of the hex string parsing may improve the code?
generally, if you have binary data (note, the hex string you only get after converting) it is a good idea to have a structure that exactly covers the stream you get from the device.

i didn't go this way in my sample code for a few reasons. first, the short members of the structure are at odd byte positions what makes it necessary to pack the structure what only can be done in a proprietary (and badly portable) way. second, all short members need to be converted from big-endian to little-endian what actually means that the binary stream is not really suitable for your current platform. the ntohs function does the byte-switch even in a portable way (if you would have the client at a big-endian system the ntohs would do nothing) what is good, but calling ntohs for 10 members explicitly is not really good code. third, you better could do the byte-swap at the original buffer in a loop, but as i wanted you to have a simple solution i posted code which extracts each short from the byte buffer (in binary mode) and  made the byte-swap. with this the structure could be spared and you would have had a similar logic to the parsing from hex string.

but you could go back to the structure and nevertheless have straight and portable code:

// the union maps the short array to the members of the structure
union DataPacket
{
      short sarr[10];
      struct {
          short roll;
          short pitch;
          short roll_rate;
          short pitch_rate;
          short yaw_rate;
          short accX;
          short accY;
          short accZ;
          short temp;
          short time;
      };
    };

    DataPacket dp = { 0 };
    size_t ns = sizeof(DataPacket)/sizeof(short);
    int cs = 0;
    for (int n = 1, k = 0; n < 21; n += 2, ++k)
    {
         short * ps = (short*)&buf[n]; // points to the first (low) byte
         dp.sarr[k] = ntohs(*ps);   // does the byte swap and assigns to union array
         // add checksum
         cs += (BYTE)buf[n];
         cs += (BYTE)buf[n+1];
    }
    if (cs%256 != (int)(buf[21]))
    {
          // error

Open in new window


the members then could be accessed like

double droll = ((double)dp.roll * 180) / 32768;

Open in new window



Sara
0
 

Author Comment

by:Marcus-Barnet
ID: 39820480
Thank you all for your suggestions!
I'm really happy to learn new things about C.
Since I have to acquire data from sensor up to 100hz, do you think it's batter struct solution over the hex parsing?
Or both solutions are good for 100hz?
0
 
LVL 32

Expert Comment

by:sarabande
ID: 39820593
do you think it's batter struct solution over the hex parsing?
in the moment you have 4 ways:

use a packed structure which contains header byte and checksum byte and pass the buffer read from device to a function by casting the buffer pointer to structure pointer. then swap each short member to little-endian by ntohs.
convert the bytes of the buffer to hex string; then extract each variable from hex string as a substring; finally convert each string variable to short variable (this last step was missing)
extract each short variable from buffer by position using function bytesToShort which additionally makes the byte-swap from big-endian to little-endian
use a union structure that maps the 10 short members to an array of short. extract each short member from buffer in a loop and assign it after byte-swap to the appropriate short element.

none of these alternatives actually does "parsing". the second method where you do a conversion to hex string is a "detour" as told by jkr. the conversions to string and from string are unnecessary. moreover it requires an additional conversion from string to (short) integer which you would need to add for each variable. the method matlab used was similar to that but matlab can implicitly convert strings to numbers and can calculate with string numbers. in c and c++ you should not try to do it that way. the last choice uses a loop to fill a structure and convert the short integers. it avoids some pitfalls of the other solutions.

nevertheless, all ways would have the same results and you should choose the one which you best understand.

you also need to add calculation for floating-point results.

Sara
0
 
LVL 86

Expert Comment

by:jkr
ID: 39823087
Well, if you're not gonna show that code to anyone, that might work ;o)
0
 

Author Comment

by:Marcus-Barnet
ID: 39825532
I do not have to show the code, fortunately, I will the only user for this code! :)

I just need it to use my old sensor and log data.
Tomorrow I'll do other tests and if everything is OK, i'll close the topic! :)
0
 

Author Comment

by:Marcus-Barnet
ID: 39861061
Thank you both of you guys!

I did long session tests with the sensor and it works very fine now with both solutions!

:)
0

Featured Post

Free Trending Threat Insights Every Day

Enhance your security with threat intelligence from the web. Get trending threat insights on hackers, exploits, and suspicious IP addresses delivered to your inbox with our free Cyber Daily.

Join & Write a Comment

Introduction We as admins face situation where we need to redirect websites to another. This may be required as a part of an upgrade keeping the old URL but website should be served from new URL. This document would brief you on different ways ca…
Summary: This tutorial covers some basics of pointer, pointer arithmetic and function pointer. What is a pointer: A pointer is a variable which holds an address. This address might be address of another variable/address of devices/address of fu…
The goal of this video is to provide viewers with basic examples to understand how to use strings and some functions related to them in the C programming language.
The goal of the video will be to teach the user the difference and consequence of passing data by value vs passing data by reference in C++. An example of passing data by value as well as an example of passing data by reference will be be given. Bot…

758 members asked questions and received personalized solutions in the past 7 days.

Join the community of 500,000 technology professionals and ask your questions.

Join & Ask a Question

Need Help in Real-Time?

Connect with top rated Experts

20 Experts available now in Live!

Get 1:1 Help Now