Go to the documentation of this file.00001 #ifndef _ROS_wiimote_State_h
00002 #define _ROS_wiimote_State_h
00003
00004 #include <stdint.h>
00005 #include <string.h>
00006 #include <stdlib.h>
00007 #include "ros/msg.h"
00008 #include "std_msgs/Header.h"
00009 #include "geometry_msgs/Vector3.h"
00010 #include "wiimote/IrSourceInfo.h"
00011 #include "ros/time.h"
00012
00013 namespace wiimote
00014 {
00015
00016 class State : public ros::Msg
00017 {
00018 public:
00019 std_msgs::Header header;
00020 geometry_msgs::Vector3 angular_velocity_zeroed;
00021 geometry_msgs::Vector3 angular_velocity_raw;
00022 float angular_velocity_covariance[9];
00023 geometry_msgs::Vector3 linear_acceleration_zeroed;
00024 geometry_msgs::Vector3 linear_acceleration_raw;
00025 float linear_acceleration_covariance[9];
00026 geometry_msgs::Vector3 nunchuk_acceleration_zeroed;
00027 geometry_msgs::Vector3 nunchuk_acceleration_raw;
00028 float nunchuk_joystick_zeroed[2];
00029 float nunchuk_joystick_raw[2];
00030 bool buttons[11];
00031 bool nunchuk_buttons[2];
00032 bool LEDs[4];
00033 bool rumble;
00034 uint8_t ir_tracking_length;
00035 wiimote::IrSourceInfo st_ir_tracking;
00036 wiimote::IrSourceInfo * ir_tracking;
00037 float raw_battery;
00038 float percent_battery;
00039 ros::Time zeroing_time;
00040 uint64_t errors;
00041 enum { INVALID = -1 };
00042 enum { INVALID_FLOAT = -1.0 };
00043 enum { MSG_BTN_1 = 0 };
00044 enum { MSG_BTN_2 = 1 };
00045 enum { MSG_BTN_A = 2 };
00046 enum { MSG_BTN_B = 3 };
00047 enum { MSG_BTN_PLUS = 4 };
00048 enum { MSG_BTN_MINUS = 5 };
00049 enum { MSG_BTN_LEFT = 6 };
00050 enum { MSG_BTN_RIGHT = 7 };
00051 enum { MSG_BTN_UP = 8 };
00052 enum { MSG_BTN_DOWN = 9 };
00053 enum { MSG_BTN_HOME = 10 };
00054 enum { MSG_BTN_Z = 0 };
00055 enum { MSG_BTN_C = 1 };
00056 enum { MSG_CLASSIC_BTN_X = 0 };
00057 enum { MSG_CLASSIC_BTN_Y = 1 };
00058 enum { MSG_CLASSIC_BTN_A = 2 };
00059 enum { MSG_CLASSIC_BTN_B = 3 };
00060 enum { MSG_CLASSIC_BTN_PLUS = 4 };
00061 enum { MSG_CLASSIC_BTN_MINUS = 5 };
00062 enum { MSG_CLASSIC_BTN_LEFT = 6 };
00063 enum { MSG_CLASSIC_BTN_RIGHT = 7 };
00064 enum { MSG_CLASSIC_BTN_UP = 8 };
00065 enum { MSG_CLASSIC_BTN_DOWN = 9 };
00066 enum { MSG_CLASSIC_BTN_HOME = 10 };
00067 enum { MSG_CLASSIC_BTN_L = 11 };
00068 enum { MSG_CLASSIC_BTN_R = 12 };
00069 enum { MSG_CLASSIC_BTN_ZL = 13 };
00070 enum { MSG_CLASSIC_BTN_ZR = 14 };
00071
00072 virtual int serialize(unsigned char *outbuffer) const
00073 {
00074 int offset = 0;
00075 offset += this->header.serialize(outbuffer + offset);
00076 offset += this->angular_velocity_zeroed.serialize(outbuffer + offset);
00077 offset += this->angular_velocity_raw.serialize(outbuffer + offset);
00078 unsigned char * angular_velocity_covariance_val = (unsigned char *) this->angular_velocity_covariance;
00079 for( uint8_t i = 0; i < 9; i++){
00080 int32_t * val_angular_velocity_covariancei = (int32_t *) &(this->angular_velocity_covariance[i]);
00081 int32_t exp_angular_velocity_covariancei = (((*val_angular_velocity_covariancei)>>23)&255);
00082 if(exp_angular_velocity_covariancei != 0)
00083 exp_angular_velocity_covariancei += 1023-127;
00084 int32_t sig_angular_velocity_covariancei = *val_angular_velocity_covariancei;
00085 *(outbuffer + offset++) = 0;
00086 *(outbuffer + offset++) = 0;
00087 *(outbuffer + offset++) = 0;
00088 *(outbuffer + offset++) = (sig_angular_velocity_covariancei<<5) & 0xff;
00089 *(outbuffer + offset++) = (sig_angular_velocity_covariancei>>3) & 0xff;
00090 *(outbuffer + offset++) = (sig_angular_velocity_covariancei>>11) & 0xff;
00091 *(outbuffer + offset++) = ((exp_angular_velocity_covariancei<<4) & 0xF0) | ((sig_angular_velocity_covariancei>>19)&0x0F);
00092 *(outbuffer + offset++) = (exp_angular_velocity_covariancei>>4) & 0x7F;
00093 if(this->angular_velocity_covariance[i] < 0) *(outbuffer + offset -1) |= 0x80;
00094 }
00095 offset += this->linear_acceleration_zeroed.serialize(outbuffer + offset);
00096 offset += this->linear_acceleration_raw.serialize(outbuffer + offset);
00097 unsigned char * linear_acceleration_covariance_val = (unsigned char *) this->linear_acceleration_covariance;
00098 for( uint8_t i = 0; i < 9; i++){
00099 int32_t * val_linear_acceleration_covariancei = (int32_t *) &(this->linear_acceleration_covariance[i]);
00100 int32_t exp_linear_acceleration_covariancei = (((*val_linear_acceleration_covariancei)>>23)&255);
00101 if(exp_linear_acceleration_covariancei != 0)
00102 exp_linear_acceleration_covariancei += 1023-127;
00103 int32_t sig_linear_acceleration_covariancei = *val_linear_acceleration_covariancei;
00104 *(outbuffer + offset++) = 0;
00105 *(outbuffer + offset++) = 0;
00106 *(outbuffer + offset++) = 0;
00107 *(outbuffer + offset++) = (sig_linear_acceleration_covariancei<<5) & 0xff;
00108 *(outbuffer + offset++) = (sig_linear_acceleration_covariancei>>3) & 0xff;
00109 *(outbuffer + offset++) = (sig_linear_acceleration_covariancei>>11) & 0xff;
00110 *(outbuffer + offset++) = ((exp_linear_acceleration_covariancei<<4) & 0xF0) | ((sig_linear_acceleration_covariancei>>19)&0x0F);
00111 *(outbuffer + offset++) = (exp_linear_acceleration_covariancei>>4) & 0x7F;
00112 if(this->linear_acceleration_covariance[i] < 0) *(outbuffer + offset -1) |= 0x80;
00113 }
00114 offset += this->nunchuk_acceleration_zeroed.serialize(outbuffer + offset);
00115 offset += this->nunchuk_acceleration_raw.serialize(outbuffer + offset);
00116 unsigned char * nunchuk_joystick_zeroed_val = (unsigned char *) this->nunchuk_joystick_zeroed;
00117 for( uint8_t i = 0; i < 2; i++){
00118 union {
00119 float real;
00120 uint32_t base;
00121 } u_nunchuk_joystick_zeroedi;
00122 u_nunchuk_joystick_zeroedi.real = this->nunchuk_joystick_zeroed[i];
00123 *(outbuffer + offset + 0) = (u_nunchuk_joystick_zeroedi.base >> (8 * 0)) & 0xFF;
00124 *(outbuffer + offset + 1) = (u_nunchuk_joystick_zeroedi.base >> (8 * 1)) & 0xFF;
00125 *(outbuffer + offset + 2) = (u_nunchuk_joystick_zeroedi.base >> (8 * 2)) & 0xFF;
00126 *(outbuffer + offset + 3) = (u_nunchuk_joystick_zeroedi.base >> (8 * 3)) & 0xFF;
00127 offset += sizeof(this->nunchuk_joystick_zeroed[i]);
00128 }
00129 unsigned char * nunchuk_joystick_raw_val = (unsigned char *) this->nunchuk_joystick_raw;
00130 for( uint8_t i = 0; i < 2; i++){
00131 union {
00132 float real;
00133 uint32_t base;
00134 } u_nunchuk_joystick_rawi;
00135 u_nunchuk_joystick_rawi.real = this->nunchuk_joystick_raw[i];
00136 *(outbuffer + offset + 0) = (u_nunchuk_joystick_rawi.base >> (8 * 0)) & 0xFF;
00137 *(outbuffer + offset + 1) = (u_nunchuk_joystick_rawi.base >> (8 * 1)) & 0xFF;
00138 *(outbuffer + offset + 2) = (u_nunchuk_joystick_rawi.base >> (8 * 2)) & 0xFF;
00139 *(outbuffer + offset + 3) = (u_nunchuk_joystick_rawi.base >> (8 * 3)) & 0xFF;
00140 offset += sizeof(this->nunchuk_joystick_raw[i]);
00141 }
00142 unsigned char * buttons_val = (unsigned char *) this->buttons;
00143 for( uint8_t i = 0; i < 11; i++){
00144 union {
00145 bool real;
00146 uint8_t base;
00147 } u_buttonsi;
00148 u_buttonsi.real = this->buttons[i];
00149 *(outbuffer + offset + 0) = (u_buttonsi.base >> (8 * 0)) & 0xFF;
00150 offset += sizeof(this->buttons[i]);
00151 }
00152 unsigned char * nunchuk_buttons_val = (unsigned char *) this->nunchuk_buttons;
00153 for( uint8_t i = 0; i < 2; i++){
00154 union {
00155 bool real;
00156 uint8_t base;
00157 } u_nunchuk_buttonsi;
00158 u_nunchuk_buttonsi.real = this->nunchuk_buttons[i];
00159 *(outbuffer + offset + 0) = (u_nunchuk_buttonsi.base >> (8 * 0)) & 0xFF;
00160 offset += sizeof(this->nunchuk_buttons[i]);
00161 }
00162 unsigned char * LEDs_val = (unsigned char *) this->LEDs;
00163 for( uint8_t i = 0; i < 4; i++){
00164 union {
00165 bool real;
00166 uint8_t base;
00167 } u_LEDsi;
00168 u_LEDsi.real = this->LEDs[i];
00169 *(outbuffer + offset + 0) = (u_LEDsi.base >> (8 * 0)) & 0xFF;
00170 offset += sizeof(this->LEDs[i]);
00171 }
00172 union {
00173 bool real;
00174 uint8_t base;
00175 } u_rumble;
00176 u_rumble.real = this->rumble;
00177 *(outbuffer + offset + 0) = (u_rumble.base >> (8 * 0)) & 0xFF;
00178 offset += sizeof(this->rumble);
00179 *(outbuffer + offset++) = ir_tracking_length;
00180 *(outbuffer + offset++) = 0;
00181 *(outbuffer + offset++) = 0;
00182 *(outbuffer + offset++) = 0;
00183 for( uint8_t i = 0; i < ir_tracking_length; i++){
00184 offset += this->ir_tracking[i].serialize(outbuffer + offset);
00185 }
00186 union {
00187 float real;
00188 uint32_t base;
00189 } u_raw_battery;
00190 u_raw_battery.real = this->raw_battery;
00191 *(outbuffer + offset + 0) = (u_raw_battery.base >> (8 * 0)) & 0xFF;
00192 *(outbuffer + offset + 1) = (u_raw_battery.base >> (8 * 1)) & 0xFF;
00193 *(outbuffer + offset + 2) = (u_raw_battery.base >> (8 * 2)) & 0xFF;
00194 *(outbuffer + offset + 3) = (u_raw_battery.base >> (8 * 3)) & 0xFF;
00195 offset += sizeof(this->raw_battery);
00196 union {
00197 float real;
00198 uint32_t base;
00199 } u_percent_battery;
00200 u_percent_battery.real = this->percent_battery;
00201 *(outbuffer + offset + 0) = (u_percent_battery.base >> (8 * 0)) & 0xFF;
00202 *(outbuffer + offset + 1) = (u_percent_battery.base >> (8 * 1)) & 0xFF;
00203 *(outbuffer + offset + 2) = (u_percent_battery.base >> (8 * 2)) & 0xFF;
00204 *(outbuffer + offset + 3) = (u_percent_battery.base >> (8 * 3)) & 0xFF;
00205 offset += sizeof(this->percent_battery);
00206 *(outbuffer + offset + 0) = (this->zeroing_time.sec >> (8 * 0)) & 0xFF;
00207 *(outbuffer + offset + 1) = (this->zeroing_time.sec >> (8 * 1)) & 0xFF;
00208 *(outbuffer + offset + 2) = (this->zeroing_time.sec >> (8 * 2)) & 0xFF;
00209 *(outbuffer + offset + 3) = (this->zeroing_time.sec >> (8 * 3)) & 0xFF;
00210 offset += sizeof(this->zeroing_time.sec);
00211 *(outbuffer + offset + 0) = (this->zeroing_time.nsec >> (8 * 0)) & 0xFF;
00212 *(outbuffer + offset + 1) = (this->zeroing_time.nsec >> (8 * 1)) & 0xFF;
00213 *(outbuffer + offset + 2) = (this->zeroing_time.nsec >> (8 * 2)) & 0xFF;
00214 *(outbuffer + offset + 3) = (this->zeroing_time.nsec >> (8 * 3)) & 0xFF;
00215 offset += sizeof(this->zeroing_time.nsec);
00216 union {
00217 uint64_t real;
00218 uint32_t base;
00219 } u_errors;
00220 u_errors.real = this->errors;
00221 *(outbuffer + offset + 0) = (u_errors.base >> (8 * 0)) & 0xFF;
00222 *(outbuffer + offset + 1) = (u_errors.base >> (8 * 1)) & 0xFF;
00223 *(outbuffer + offset + 2) = (u_errors.base >> (8 * 2)) & 0xFF;
00224 *(outbuffer + offset + 3) = (u_errors.base >> (8 * 3)) & 0xFF;
00225 offset += sizeof(this->errors);
00226 return offset;
00227 }
00228
00229 virtual int deserialize(unsigned char *inbuffer)
00230 {
00231 int offset = 0;
00232 offset += this->header.deserialize(inbuffer + offset);
00233 offset += this->angular_velocity_zeroed.deserialize(inbuffer + offset);
00234 offset += this->angular_velocity_raw.deserialize(inbuffer + offset);
00235 uint8_t * angular_velocity_covariance_val = (uint8_t*) this->angular_velocity_covariance;
00236 for( uint8_t i = 0; i < 9; i++){
00237 uint32_t * val_angular_velocity_covariancei = (uint32_t*) &(this->angular_velocity_covariance[i]);
00238 offset += 3;
00239 *val_angular_velocity_covariancei = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00240 *val_angular_velocity_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00241 *val_angular_velocity_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00242 *val_angular_velocity_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00243 uint32_t exp_angular_velocity_covariancei = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00244 exp_angular_velocity_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00245 if(exp_angular_velocity_covariancei !=0)
00246 *val_angular_velocity_covariancei |= ((exp_angular_velocity_covariancei)-1023+127)<<23;
00247 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->angular_velocity_covariance[i] = -this->angular_velocity_covariance[i];
00248 }
00249 offset += this->linear_acceleration_zeroed.deserialize(inbuffer + offset);
00250 offset += this->linear_acceleration_raw.deserialize(inbuffer + offset);
00251 uint8_t * linear_acceleration_covariance_val = (uint8_t*) this->linear_acceleration_covariance;
00252 for( uint8_t i = 0; i < 9; i++){
00253 uint32_t * val_linear_acceleration_covariancei = (uint32_t*) &(this->linear_acceleration_covariance[i]);
00254 offset += 3;
00255 *val_linear_acceleration_covariancei = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00256 *val_linear_acceleration_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00257 *val_linear_acceleration_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00258 *val_linear_acceleration_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00259 uint32_t exp_linear_acceleration_covariancei = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00260 exp_linear_acceleration_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00261 if(exp_linear_acceleration_covariancei !=0)
00262 *val_linear_acceleration_covariancei |= ((exp_linear_acceleration_covariancei)-1023+127)<<23;
00263 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->linear_acceleration_covariance[i] = -this->linear_acceleration_covariance[i];
00264 }
00265 offset += this->nunchuk_acceleration_zeroed.deserialize(inbuffer + offset);
00266 offset += this->nunchuk_acceleration_raw.deserialize(inbuffer + offset);
00267 uint8_t * nunchuk_joystick_zeroed_val = (uint8_t*) this->nunchuk_joystick_zeroed;
00268 for( uint8_t i = 0; i < 2; i++){
00269 union {
00270 float real;
00271 uint32_t base;
00272 } u_nunchuk_joystick_zeroedi;
00273 u_nunchuk_joystick_zeroedi.base = 0;
00274 u_nunchuk_joystick_zeroedi.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00275 u_nunchuk_joystick_zeroedi.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00276 u_nunchuk_joystick_zeroedi.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00277 u_nunchuk_joystick_zeroedi.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00278 this->nunchuk_joystick_zeroed[i] = u_nunchuk_joystick_zeroedi.real;
00279 offset += sizeof(this->nunchuk_joystick_zeroed[i]);
00280 }
00281 uint8_t * nunchuk_joystick_raw_val = (uint8_t*) this->nunchuk_joystick_raw;
00282 for( uint8_t i = 0; i < 2; i++){
00283 union {
00284 float real;
00285 uint32_t base;
00286 } u_nunchuk_joystick_rawi;
00287 u_nunchuk_joystick_rawi.base = 0;
00288 u_nunchuk_joystick_rawi.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00289 u_nunchuk_joystick_rawi.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00290 u_nunchuk_joystick_rawi.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00291 u_nunchuk_joystick_rawi.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00292 this->nunchuk_joystick_raw[i] = u_nunchuk_joystick_rawi.real;
00293 offset += sizeof(this->nunchuk_joystick_raw[i]);
00294 }
00295 uint8_t * buttons_val = (uint8_t*) this->buttons;
00296 for( uint8_t i = 0; i < 11; i++){
00297 union {
00298 bool real;
00299 uint8_t base;
00300 } u_buttonsi;
00301 u_buttonsi.base = 0;
00302 u_buttonsi.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00303 this->buttons[i] = u_buttonsi.real;
00304 offset += sizeof(this->buttons[i]);
00305 }
00306 uint8_t * nunchuk_buttons_val = (uint8_t*) this->nunchuk_buttons;
00307 for( uint8_t i = 0; i < 2; i++){
00308 union {
00309 bool real;
00310 uint8_t base;
00311 } u_nunchuk_buttonsi;
00312 u_nunchuk_buttonsi.base = 0;
00313 u_nunchuk_buttonsi.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00314 this->nunchuk_buttons[i] = u_nunchuk_buttonsi.real;
00315 offset += sizeof(this->nunchuk_buttons[i]);
00316 }
00317 uint8_t * LEDs_val = (uint8_t*) this->LEDs;
00318 for( uint8_t i = 0; i < 4; i++){
00319 union {
00320 bool real;
00321 uint8_t base;
00322 } u_LEDsi;
00323 u_LEDsi.base = 0;
00324 u_LEDsi.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00325 this->LEDs[i] = u_LEDsi.real;
00326 offset += sizeof(this->LEDs[i]);
00327 }
00328 union {
00329 bool real;
00330 uint8_t base;
00331 } u_rumble;
00332 u_rumble.base = 0;
00333 u_rumble.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00334 this->rumble = u_rumble.real;
00335 offset += sizeof(this->rumble);
00336 uint8_t ir_tracking_lengthT = *(inbuffer + offset++);
00337 if(ir_tracking_lengthT > ir_tracking_length)
00338 this->ir_tracking = (wiimote::IrSourceInfo*)realloc(this->ir_tracking, ir_tracking_lengthT * sizeof(wiimote::IrSourceInfo));
00339 offset += 3;
00340 ir_tracking_length = ir_tracking_lengthT;
00341 for( uint8_t i = 0; i < ir_tracking_length; i++){
00342 offset += this->st_ir_tracking.deserialize(inbuffer + offset);
00343 memcpy( &(this->ir_tracking[i]), &(this->st_ir_tracking), sizeof(wiimote::IrSourceInfo));
00344 }
00345 union {
00346 float real;
00347 uint32_t base;
00348 } u_raw_battery;
00349 u_raw_battery.base = 0;
00350 u_raw_battery.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00351 u_raw_battery.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00352 u_raw_battery.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00353 u_raw_battery.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00354 this->raw_battery = u_raw_battery.real;
00355 offset += sizeof(this->raw_battery);
00356 union {
00357 float real;
00358 uint32_t base;
00359 } u_percent_battery;
00360 u_percent_battery.base = 0;
00361 u_percent_battery.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00362 u_percent_battery.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00363 u_percent_battery.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00364 u_percent_battery.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00365 this->percent_battery = u_percent_battery.real;
00366 offset += sizeof(this->percent_battery);
00367 this->zeroing_time.sec = ((uint32_t) (*(inbuffer + offset)));
00368 this->zeroing_time.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00369 this->zeroing_time.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00370 this->zeroing_time.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00371 offset += sizeof(this->zeroing_time.sec);
00372 this->zeroing_time.nsec = ((uint32_t) (*(inbuffer + offset)));
00373 this->zeroing_time.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00374 this->zeroing_time.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00375 this->zeroing_time.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00376 offset += sizeof(this->zeroing_time.nsec);
00377 union {
00378 uint64_t real;
00379 uint32_t base;
00380 } u_errors;
00381 u_errors.base = 0;
00382 u_errors.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00383 u_errors.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00384 u_errors.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00385 u_errors.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00386 this->errors = u_errors.real;
00387 offset += sizeof(this->errors);
00388 return offset;
00389 }
00390
00391 const char * getType(){ return "wiimote/State"; };
00392 const char * getMD5(){ return "a69651e8129655c6ed3c5039e468362c"; };
00393
00394 };
00395
00396 }
00397 #endif