Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include <stdio.h>
00024 #include <errno.h>
00025 #include <string.h>
00026 #include <p2os_driver/packet.hpp>
00027 #include <unistd.h>
00028 #include <stdlib.h>
00029 #include <ros/ros.h>
00030
00031 void P2OSPacket::Print()
00032 {
00033 if (packet) {
00034 ROS_INFO("\"");
00035 for (int i = 0; i < size; i++) {
00036 ROS_INFO("%u ", packet[i]);
00037 }
00038 ROS_INFO("\"");
00039 }
00040 }
00041
00042 void P2OSPacket::PrintHex()
00043 {
00044 if (packet) {
00045 ROS_INFO("\"");
00046 for (int i = 0; i < size; i++) {
00047 ROS_INFO("0x%.2x ", packet[i]);
00048 }
00049 ROS_INFO("\"");
00050 }
00051 }
00052
00053
00054 bool P2OSPacket::Check()
00055 {
00056 const int16_t chksum = CalcChkSum();
00057 return (chksum == (packet[size - 2] << 8)) | packet[size - 1];
00058 }
00059
00060 int P2OSPacket::CalcChkSum()
00061 {
00062 unsigned char * buffer = &packet[3];
00063 int c = 0;
00064 int n;
00065
00066 for (n = size - 5; n > 1; ) {
00067 c += (*(buffer) << 8) | *(buffer + 1);
00068 c = c & 0xffff;
00069 n -= 2;
00070 buffer += 2;
00071 }
00072 if (n > 0) {
00073 c ^= static_cast<int>(*(buffer++));
00074 }
00075
00076 return c;
00077 }
00078
00079 int P2OSPacket::Receive(int fd)
00080 {
00081 unsigned char prefix[3];
00082 int cnt;
00083
00084 ::memset(packet, 0, sizeof(packet));
00085
00086 do {
00087 ::memset(prefix, 0, sizeof(prefix));
00088
00089 while (1) {
00090 cnt = 0;
00091 while (cnt != 1) {
00092 if ((cnt += read(fd, &prefix[2], 1)) < 0) {
00093 ROS_ERROR(
00094 "Error reading packet.hppeader from robot connection: P2OSPacket():Receive():read():");
00095 return 1;
00096 }
00097 }
00098
00099 if (prefix[0] == 0xFA && prefix[1] == 0xFB) {
00100 break;
00101 }
00102
00103 timestamp = ros::Time::now();
00104
00105
00106
00107 prefix[0] = prefix[1];
00108 prefix[1] = prefix[2];
00109
00110 }
00111
00112
00113 size = prefix[2] + 3;
00114 memcpy(packet, prefix, 3);
00115
00116 cnt = 0;
00117 while (cnt != prefix[2]) {
00118 if ((cnt += read(fd, &packet[3 + cnt], prefix[2] - cnt)) < 0) {
00119 ROS_ERROR(
00120 "Error reading packet body from robot connection: P2OSPacket():Receive():read():");
00121 return 1;
00122 }
00123 }
00124 } while (!Check());
00125 return 0;
00126 }
00127
00128 int P2OSPacket::Build(unsigned char * data, unsigned char datasize)
00129 {
00130 int16_t chksum;
00131
00132 size = datasize + 5;
00133
00134
00135 packet[0] = 0xFA;
00136 packet[1] = 0xFB;
00137
00138 if (size > 198) {
00139 ROS_ERROR("Packet to P2OS can't be larger than 200 bytes");
00140 return 1;
00141 }
00142 packet[2] = datasize + 2;
00143
00144 memcpy(&packet[3], data, datasize);
00145
00146 chksum = CalcChkSum();
00147 packet[3 + datasize] = chksum >> 8;
00148 packet[3 + datasize + 1] = chksum & 0xFF;
00149
00150 if (!Check()) {
00151 ROS_ERROR("DAMN");
00152 return 1;
00153 }
00154 return 0;
00155 }
00156
00157 int P2OSPacket::Send(int fd)
00158 {
00159 int cnt = 0;
00160
00161 while (cnt != size) {
00162 if ((cnt += write(fd, packet, size)) < 0) {
00163 ROS_ERROR("Send");
00164 return 1;
00165 }
00166 }
00167 return 0;
00168 }