three_axis_gyro.hpp
Go to the documentation of this file.
00001 
00009 /*****************************************************************************
00010 ** Preprocessor
00011 *****************************************************************************/
00012 
00013 #ifndef KOBUKI_THREE_AXIS_GYRO_DATA_HPP__
00014 #define KOBUKI_THREE_AXIS_GYRO_DATA_HPP__
00015 
00016 /*****************************************************************************
00017 ** Includes
00018 *****************************************************************************/
00019 
00020 #include "../packet_handler/payload_base.hpp"
00021 #include "../packet_handler/payload_headers.hpp"
00022 
00023 /*****************************************************************************
00024 ** Defines
00025 *****************************************************************************/
00026 
00027 #define MAX_DATA_SIZE (3*8) //derived from ST_GYRO_MAX_DATA_SIZE in firmware
00028 
00029 /*****************************************************************************
00030 ** Namespaces
00031 *****************************************************************************/
00032 
00033 namespace kobuki
00034 {
00035 
00036 /*****************************************************************************
00037 ** Interface
00038 *****************************************************************************/
00039 
00040 class ThreeAxisGyro : public packet_handler::payloadBase
00041 {
00042 public:
00043   ThreeAxisGyro() : packet_handler::payloadBase(true, 4) {};
00044   struct Data {
00045     unsigned char frame_id;
00046     unsigned char followed_data_length;
00047     unsigned short data[MAX_DATA_SIZE];
00048   } data;
00049 
00050   virtual ~ThreeAxisGyro() {};
00051 
00052   bool serialise(ecl::PushAndPop<unsigned char> & byteStream)
00053   {
00054     unsigned char length = 2 + 2 * data.followed_data_length;
00055     buildBytes(Header::ThreeAxisGyro, byteStream);
00056     buildBytes(length, byteStream);
00057     buildBytes(data.frame_id, byteStream);
00058     buildBytes(data.followed_data_length, byteStream);
00059     for (unsigned int i=0; i<data.followed_data_length; i++)
00060       buildBytes(data.data[i], byteStream);
00061     return true;
00062   }
00063 
00064   bool deserialise(ecl::PushAndPop<unsigned char> & byteStream)
00065   {
00066     if (byteStream.size() < length+2)
00067     {
00068       //std::cout << "kobuki_node: three_axis_gyro: deserialise failed. not enough byte stream." << std::endl;
00069       return false;
00070     }
00071 
00072     unsigned char header_id, length_packed;
00073     buildVariable(header_id, byteStream);
00074     buildVariable(length_packed, byteStream);
00075     if( header_id != Header::ThreeAxisGyro ) return false;
00076     if( length > length_packed ) return false;
00077 
00078     buildVariable(data.frame_id, byteStream);
00079     buildVariable(data.followed_data_length, byteStream);
00080     if( length_packed != 2 + 2 * data.followed_data_length ) return false;
00081 
00082     for (unsigned int i=0; i<data.followed_data_length; i++)
00083       buildVariable(data.data[i], byteStream);
00084 
00085     //showMe();
00086     return constrain();
00087   }
00088 
00089   bool constrain()
00090   {
00091     return true;
00092   }
00093 
00094   void showMe()
00095   {
00096   }
00097 };
00098 
00099 } // namespace kobuki
00100 
00101 #endif /* KOBUKI_THREE_AXIS_GYRO_DATA_HPP__ */
00102 


kobuki_driver
Author(s): Daniel Stonier , Younghun Ju , Jorge Santos Simon
autogenerated on Thu Jun 6 2019 20:24:37