/quad2/arduino/src/ros_lib/geometry_msgs/Vector3.h - Annotate - Quadrotor - Robotics Club

c1426757

Tom Mullins

#ifndef _ROS_geometry_msgs_Vector3_h

2

#define _ROS_geometry_msgs_Vector3_h

3

4

#include <stdint.h>

5

#include <string.h>

6

#include <stdlib.h>

7

#include "ros/msg.h"

8

9

namespace geometry_msgs

10

11

12

  class Vector3 : public ros::Msg

13

14

    public:

15

      float x;

16

      float y;

17

      float z;

18

19

    virtual int serialize(unsigned char *outbuffer) const

20

21

      int offset = 0;

22

      int32_t * val_x = (long *) &(this->x);

23

      int32_t exp_x = (((*val_x)>>23)&255);

24

      if(exp_x != 0)

25

        exp_x += 1023-127;

26

      int32_t sig_x = *val_x;

27

      *(outbuffer + offset++) = 0;

28

      *(outbuffer + offset++) = 0;

29

      *(outbuffer + offset++) = 0;

30

      *(outbuffer + offset++) = (sig_x<<5) & 0xff;

31

      *(outbuffer + offset++) = (sig_x>>3) & 0xff;

32

      *(outbuffer + offset++) = (sig_x>>11) & 0xff;

33

      *(outbuffer + offset++) = ((exp_x<<4) & 0xF0) | ((sig_x>>19)&0x0F);

34

      *(outbuffer + offset++) = (exp_x>>4) & 0x7F;

35

      if(this->x < 0) *(outbuffer + offset -1) |= 0x80;

36

      int32_t * val_y = (long *) &(this->y);

37

      int32_t exp_y = (((*val_y)>>23)&255);

38

      if(exp_y != 0)

39

        exp_y += 1023-127;

40

      int32_t sig_y = *val_y;

41

      *(outbuffer + offset++) = 0;

42

      *(outbuffer + offset++) = 0;

43

      *(outbuffer + offset++) = 0;

44

      *(outbuffer + offset++) = (sig_y<<5) & 0xff;

45

      *(outbuffer + offset++) = (sig_y>>3) & 0xff;

46

      *(outbuffer + offset++) = (sig_y>>11) & 0xff;

47

      *(outbuffer + offset++) = ((exp_y<<4) & 0xF0) | ((sig_y>>19)&0x0F);

48

      *(outbuffer + offset++) = (exp_y>>4) & 0x7F;

49

      if(this->y < 0) *(outbuffer + offset -1) |= 0x80;

50

      int32_t * val_z = (long *) &(this->z);

51

      int32_t exp_z = (((*val_z)>>23)&255);

52

      if(exp_z != 0)

53

        exp_z += 1023-127;

54

      int32_t sig_z = *val_z;

55

      *(outbuffer + offset++) = 0;

56

      *(outbuffer + offset++) = 0;

57

      *(outbuffer + offset++) = 0;

58

      *(outbuffer + offset++) = (sig_z<<5) & 0xff;

59

      *(outbuffer + offset++) = (sig_z>>3) & 0xff;

60

      *(outbuffer + offset++) = (sig_z>>11) & 0xff;

61

      *(outbuffer + offset++) = ((exp_z<<4) & 0xF0) | ((sig_z>>19)&0x0F);

62

      *(outbuffer + offset++) = (exp_z>>4) & 0x7F;

63

      if(this->z < 0) *(outbuffer + offset -1) |= 0x80;

64

      return offset;

65

66

67

    virtual int deserialize(unsigned char *inbuffer)

68

69

      int offset = 0;

70

      uint32_t * val_x = (uint32_t*) &(this->x);

71

      offset += 3;

72

      *val_x = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);

73

      *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;

74

      *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;

75

      *val_x |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;

76

      uint32_t exp_x = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;

77

      exp_x |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;

78

      if(exp_x !=0)

79

        *val_x |= ((exp_x)-1023+127)<<23;

80

      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->x = -this->x;

81

      uint32_t * val_y = (uint32_t*) &(this->y);

82

      offset += 3;

83

      *val_y = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);

84

      *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;

85

      *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;

86

      *val_y |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;

87

      uint32_t exp_y = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;

88

      exp_y |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;

89

      if(exp_y !=0)

90

        *val_y |= ((exp_y)-1023+127)<<23;

91

      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->y = -this->y;

92

      uint32_t * val_z = (uint32_t*) &(this->z);

93

      offset += 3;

94

      *val_z = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);

95

      *val_z |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;

96

      *val_z |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;

97

      *val_z |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;

98

      uint32_t exp_z = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;

99

      exp_z |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;

100

      if(exp_z !=0)

101

        *val_z |= ((exp_z)-1023+127)<<23;

102

      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->z = -this->z;

103

     return offset;

104

105

106

    const char * getType(){ return "geometry_msgs/Vector3"; };

107

    const char * getMD5(){ return "4a842b65f413084dc2b10fb484ea7f17"; };

108

109

};

110

111

112

#endif

Project

General

Profile

Quadrotor

root / quad2 / arduino / src / ros_lib / geometry_msgs / Vector3.h @ c1426757