FreeD.h

00001 #ifndef __FreeD_h__
00002 #define __FreeD_h__
00003 
00004 #include "mbed.h"
00005 #include <cstdint>
00006 
00007 struct D1MsgFormat_s {
00008     uint8_t header; //0xD1, annoying it cant be set as default value here!
00009     uint8_t id; //camera ID, use 255 minus Rover ID
00010     uint8_t yaw[3];
00011     uint8_t pitch[3];
00012     uint8_t roll[3];
00013     uint8_t x[3];
00014     uint8_t y[3];
00015     uint8_t z[3];
00016     uint8_t zoom[3];
00017     uint8_t focus[3];
00018     uint16_t spare;
00019     uint8_t checksum;
00020 } __attribute__((packed)) ;
00021 
00022 
00023 inline void set24bitValue(uint8_t *target, int32_t value)
00024 {
00025     *target = (uint8_t) ((value>>16) & 0xff);
00026     *(target+1) = (uint8_t) ((value>>8) & 0xff);
00027     *(target+2) = (uint8_t) (value & 0xff);
00028 }
00029 
00030 inline int GetFdCRC(void *data) {
00031     uint8_t *dataPtr = (uint8_t *)data;
00032     //uint8_t *crcPtr = (uint8_t *)checksum;
00033     int len = sizeof(D1MsgFormat_s) -1;
00034     uint8_t sum =0;
00035     int byteCount = 0;
00036 
00037     while ((len--) > 0) {sum += dataPtr[byteCount++];}
00038         
00039     //crcPtr[0] = 0x40 - sum;
00040     return 0x40 - sum;
00041 }
00042 
00043 
00044 #endif