three_axis_gyro.hpp
Go to the documentation of this file.
1 
9 /*****************************************************************************
10 ** Preprocessor
11 *****************************************************************************/
12 
13 #ifndef KOBUKI_THREE_AXIS_GYRO_DATA_HPP__
14 #define KOBUKI_THREE_AXIS_GYRO_DATA_HPP__
15 
16 /*****************************************************************************
17 ** Includes
18 *****************************************************************************/
19 
20 #include "../packet_handler/payload_base.hpp"
21 #include "../packet_handler/payload_headers.hpp"
22 
23 /*****************************************************************************
24 ** Defines
25 *****************************************************************************/
26 
27 #define MAX_DATA_SIZE (3*8) //derived from ST_GYRO_MAX_DATA_SIZE in firmware
28 
29 /*****************************************************************************
30 ** Namespaces
31 *****************************************************************************/
32 
33 namespace kobuki
34 {
35 
36 /*****************************************************************************
37 ** Interface
38 *****************************************************************************/
39 
41 {
42 public:
44  struct Data {
45  unsigned char frame_id;
46  unsigned char followed_data_length;
47  unsigned short data[MAX_DATA_SIZE];
48  } data;
49 
50  virtual ~ThreeAxisGyro() {};
51 
53  {
54  unsigned char length = 2 + 2 * data.followed_data_length;
55  buildBytes(Header::ThreeAxisGyro, byteStream);
56  buildBytes(length, byteStream);
57  buildBytes(data.frame_id, byteStream);
58  buildBytes(data.followed_data_length, byteStream);
59  for (unsigned int i=0; i<data.followed_data_length; i++)
60  buildBytes(data.data[i], byteStream);
61  return true;
62  }
63 
65  {
66  if (byteStream.size() < length+2)
67  {
68  //std::cout << "kobuki_node: three_axis_gyro: deserialise failed. not enough byte stream." << std::endl;
69  return false;
70  }
71 
72  unsigned char header_id, length_packed;
73  buildVariable(header_id, byteStream);
74  buildVariable(length_packed, byteStream);
75  if( header_id != Header::ThreeAxisGyro ) return false;
76  if( length > length_packed ) return false;
77 
78  buildVariable(data.frame_id, byteStream);
79  buildVariable(data.followed_data_length, byteStream);
80  if( length_packed != 2 + 2 * data.followed_data_length ) return false;
81 
82  for (unsigned int i=0; i<data.followed_data_length; i++)
83  buildVariable(data.data[i], byteStream);
84 
85  //showMe();
86  return constrain();
87  }
88 
89  bool constrain()
90  {
91  return true;
92  }
93 
94  void showMe()
95  {
96  }
97 };
98 
99 } // namespace kobuki
100 
101 #endif /* KOBUKI_THREE_AXIS_GYRO_DATA_HPP__ */
102 
bool serialise(ecl::PushAndPop< unsigned char > &byteStream)
unsigned int size() const
const unsigned char length
bool deserialise(ecl::PushAndPop< unsigned char > &byteStream)
Provides base class for payloads.
payloadBase(const bool is_dynamic_=false, const unsigned char length_=0)
void buildVariable(T &V, ecl::PushAndPop< unsigned char > &buffer)
#define MAX_DATA_SIZE
unsigned short data[MAX_DATA_SIZE]
void buildBytes(const T &V, ecl::PushAndPop< unsigned char > &buffer)


kobuki_driver
Author(s): Daniel Stonier , Younghun Ju , Jorge Santos Simon
autogenerated on Fri Sep 18 2020 03:22:02