modify for Hydro version

Dependencies:   MODSERIAL

Fork of rosserial_mbed_lib by nucho

Committer:
nucho
Date:
Sun Oct 16 07:19:36 2011 +0000
Revision:
1:ff0ec969dad1
Parent:
0:77afd7560544
Child:
3:1cf99502f396
This program supported the revision of 143 of rosserial.
And the bug fix of receive of array data.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nucho 0:77afd7560544 1 #ifndef ros_Float64_h
nucho 0:77afd7560544 2 #define ros_Float64_h
nucho 0:77afd7560544 3
nucho 0:77afd7560544 4 #include <stdint.h>
nucho 0:77afd7560544 5 #include <string.h>
nucho 0:77afd7560544 6 #include <stdlib.h>
nucho 0:77afd7560544 7 #include "../ros/msg.h"
nucho 0:77afd7560544 8
nucho 0:77afd7560544 9 namespace std_msgs
nucho 0:77afd7560544 10 {
nucho 0:77afd7560544 11
nucho 0:77afd7560544 12 class Float64 : public ros::Msg
nucho 0:77afd7560544 13 {
nucho 0:77afd7560544 14 public:
nucho 1:ff0ec969dad1 15 double data;
nucho 0:77afd7560544 16
nucho 0:77afd7560544 17 virtual int serialize(unsigned char *outbuffer)
nucho 0:77afd7560544 18 {
nucho 0:77afd7560544 19 int offset = 0;
nucho 1:ff0ec969dad1 20 union {
nucho 1:ff0ec969dad1 21 double real;
nucho 1:ff0ec969dad1 22 uint64_t base;
nucho 1:ff0ec969dad1 23 } u_data;
nucho 1:ff0ec969dad1 24 u_data.real = this->data;
nucho 1:ff0ec969dad1 25 *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF;
nucho 1:ff0ec969dad1 26 *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF;
nucho 1:ff0ec969dad1 27 *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF;
nucho 1:ff0ec969dad1 28 *(outbuffer + offset + 3) = (u_data.base >> (8 * 3)) & 0xFF;
nucho 1:ff0ec969dad1 29 *(outbuffer + offset + 4) = (u_data.base >> (8 * 4)) & 0xFF;
nucho 1:ff0ec969dad1 30 *(outbuffer + offset + 5) = (u_data.base >> (8 * 5)) & 0xFF;
nucho 1:ff0ec969dad1 31 *(outbuffer + offset + 6) = (u_data.base >> (8 * 6)) & 0xFF;
nucho 1:ff0ec969dad1 32 *(outbuffer + offset + 7) = (u_data.base >> (8 * 7)) & 0xFF;
nucho 1:ff0ec969dad1 33 offset += sizeof(this->data);
nucho 1:ff0ec969dad1 34 return offset;
nucho 1:ff0ec969dad1 35 }
nucho 1:ff0ec969dad1 36
nucho 1:ff0ec969dad1 37 virtual int deserialize(unsigned char *inbuffer)
nucho 1:ff0ec969dad1 38 {
nucho 1:ff0ec969dad1 39 int offset = 0;
nucho 1:ff0ec969dad1 40 union {
nucho 1:ff0ec969dad1 41 double real;
nucho 1:ff0ec969dad1 42 uint64_t base;
nucho 1:ff0ec969dad1 43 } u_data;
nucho 1:ff0ec969dad1 44 u_data.base = 0;
nucho 1:ff0ec969dad1 45 u_data.base |= ((typeof(u_data.base)) (*(inbuffer + offset + 0))) << (8 * 0);
nucho 1:ff0ec969dad1 46 u_data.base |= ((typeof(u_data.base)) (*(inbuffer + offset + 1))) << (8 * 1);
nucho 1:ff0ec969dad1 47 u_data.base |= ((typeof(u_data.base)) (*(inbuffer + offset + 2))) << (8 * 2);
nucho 1:ff0ec969dad1 48 u_data.base |= ((typeof(u_data.base)) (*(inbuffer + offset + 3))) << (8 * 3);
nucho 1:ff0ec969dad1 49 u_data.base |= ((typeof(u_data.base)) (*(inbuffer + offset + 4))) << (8 * 4);
nucho 1:ff0ec969dad1 50 u_data.base |= ((typeof(u_data.base)) (*(inbuffer + offset + 5))) << (8 * 5);
nucho 1:ff0ec969dad1 51 u_data.base |= ((typeof(u_data.base)) (*(inbuffer + offset + 6))) << (8 * 6);
nucho 1:ff0ec969dad1 52 u_data.base |= ((typeof(u_data.base)) (*(inbuffer + offset + 7))) << (8 * 7);
nucho 1:ff0ec969dad1 53 this->data = u_data.real;
nucho 1:ff0ec969dad1 54 offset += sizeof(this->data);
nucho 1:ff0ec969dad1 55 return offset;
nucho 1:ff0ec969dad1 56 }
nucho 1:ff0ec969dad1 57 /*
nucho 1:ff0ec969dad1 58 virtual int serialize(unsigned char *outbuffer)
nucho 1:ff0ec969dad1 59 {
nucho 1:ff0ec969dad1 60 int offset = 0;
nucho 0:77afd7560544 61 long * val_data = (long *) &(this->data);
nucho 0:77afd7560544 62 long exp_data = (((*val_data)>>23)&255);
nucho 0:77afd7560544 63 if(exp_data != 0)
nucho 0:77afd7560544 64 exp_data += 1023-127;
nucho 0:77afd7560544 65 long sig_data = *val_data;
nucho 0:77afd7560544 66 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 67 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 68 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 69 *(outbuffer + offset++) = (sig_data<<5) & 0xff;
nucho 0:77afd7560544 70 *(outbuffer + offset++) = (sig_data>>3) & 0xff;
nucho 0:77afd7560544 71 *(outbuffer + offset++) = (sig_data>>11) & 0xff;
nucho 0:77afd7560544 72 *(outbuffer + offset++) = ((exp_data<<4) & 0xF0) | ((sig_data>>19)&0x0F);
nucho 0:77afd7560544 73 *(outbuffer + offset++) = (exp_data>>4) & 0x7F;
nucho 0:77afd7560544 74 if(this->data < 0) *(outbuffer + offset -1) |= 0x80;
nucho 0:77afd7560544 75 return offset;
nucho 0:77afd7560544 76 }
nucho 0:77afd7560544 77
nucho 0:77afd7560544 78 virtual int deserialize(unsigned char *inbuffer)
nucho 0:77afd7560544 79 {
nucho 0:77afd7560544 80 int offset = 0;
nucho 0:77afd7560544 81 unsigned long * val_data = (unsigned long*) &(this->data);
nucho 0:77afd7560544 82 offset += 3;
nucho 0:77afd7560544 83 *val_data = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
nucho 0:77afd7560544 84 *val_data |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
nucho 0:77afd7560544 85 *val_data |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
nucho 0:77afd7560544 86 *val_data |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
nucho 0:77afd7560544 87 unsigned long exp_data = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
nucho 0:77afd7560544 88 exp_data |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
nucho 0:77afd7560544 89 if(exp_data !=0)
nucho 0:77afd7560544 90 *val_data |= ((exp_data)-1023+127)<<23;
nucho 0:77afd7560544 91 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->data = -this->data;
nucho 0:77afd7560544 92 return offset;
nucho 0:77afd7560544 93 }
nucho 1:ff0ec969dad1 94 */
nucho 0:77afd7560544 95 virtual const char * getType(){ return "std_msgs/Float64"; };
nucho 0:77afd7560544 96
nucho 0:77afd7560544 97 };
nucho 0:77afd7560544 98
nucho 0:77afd7560544 99 }
nucho 0:77afd7560544 100 #endif