00001 // Copyright (C) 2004 Pablo Mejia 00002 00003 00004 #ifndef NCOM_PACKET_H 00005 #define NCOM_PACKET_H 00006 00007 00008 #include "endian.H" 00009 00010 00011 namespace ncom 00012 { 00013 using namespace endian; 00014 00015 static inline s32_t s24 (unsigned char const *data) 00016 { 00017 s32_t v = (letoh<u32_t> (data) << 8); 00018 00019 return (v >> 8); 00020 } 00021 00022 class packet_t 00023 { 00024 protected: 00025 static unsigned const PACKET_SIZE = 72; 00026 00027 static unsigned const STX_OFFSET = 0; 00028 static unsigned const TIME_OFFSET = 1; 00029 static unsigned const X_ACCELERATION_OFFSET = 3; 00030 static unsigned const Y_ACCELERATION_OFFSET = 6; 00031 static unsigned const Z_ACCELERATION_OFFSET = 9; 00032 static unsigned const X_ANGULAR_RATE_OFFSET = 12; 00033 static unsigned const Y_ANGULAR_RATE_OFFSET = 15; 00034 static unsigned const Z_ANGULAR_RATE_OFFSET = 18; 00035 static unsigned const NAV_STATUS_OFFSET = 21; 00036 static unsigned const CHECKSUM_1_OFFSET = 22; 00037 00038 static unsigned const LATITUDE_OFFSET = 23; 00039 static unsigned const LONGITUDE_OFFSET = 31; 00040 static unsigned const ALTITUDE_OFFSET = 39; 00041 static unsigned const N_VELOCITY_OFFSET = 43; 00042 static unsigned const E_VELOCITY_OFFSET = 46; 00043 static unsigned const D_VELOCITY_OFFSET = 49; 00044 static unsigned const HEADING_OFFSET = 52; 00045 static unsigned const PITCH_OFFSET = 55; 00046 static unsigned const ROLL_OFFSET = 58; 00047 static unsigned const CHECKSUM_2_OFFSET = 61; 00048 00049 static unsigned const CHANNEL_OFFSET = 62; 00050 static unsigned const CHANNEL_DATA_OFFSET = 63; 00051 static unsigned const CHECKSUM_3_OFFSET = 71; 00052 00053 bool m_valid; 00054 unsigned char const *m_data; 00055 00056 static unsigned calc_crc (unsigned char const *data, 00057 unsigned size); 00058 static unsigned skip_to_stx (unsigned char const *data, 00059 unsigned size); 00060 00061 public: 00062 static unsigned const STX = 0xE7; 00063 00064 packet_t (); 00065 packet_t (packet_t const *packet); 00066 00067 unsigned decode (unsigned char const *buf, unsigned buf_size); 00068 00069 bool valid (void) const { return (m_valid); } 00070 00071 unsigned stx (void) const 00072 { 00073 return (m_data[STX_OFFSET]); 00074 } 00075 double time (void) const 00076 { 00077 return (1E-3 * letoh<u16_t> (m_data + TIME_OFFSET)); 00078 } 00079 double x_acceleration (void) const 00080 { 00081 return (1E-4 * s24 (m_data + X_ACCELERATION_OFFSET)); 00082 } 00083 double y_acceleration (void) const 00084 { 00085 return (1E-4 * s24 (m_data + Y_ACCELERATION_OFFSET)); 00086 } 00087 double z_acceleration (void) const 00088 { 00089 return (1E-4 * s24 (m_data + Z_ACCELERATION_OFFSET)); 00090 } 00091 double x_angular_rate (void) const 00092 { 00093 return (1E-5 * s24 (m_data + X_ANGULAR_RATE_OFFSET)); 00094 } 00095 double y_angular_rate (void) const 00096 { 00097 return (1E-5 * s24 (m_data + Y_ANGULAR_RATE_OFFSET)); 00098 } 00099 double z_angular_rate (void) const 00100 { 00101 return (1E-5 * s24 (m_data + Z_ANGULAR_RATE_OFFSET)); 00102 } 00103 enum nav_status_t 00104 { 00105 NAV_STATUS_INVALID, 00106 NAV_STATUS_RAW_IMU, 00107 NAV_STATUS_INTIALIZING, 00108 NAV_STATUS_LOCKING, 00109 NAV_STATUS_LOCKED, 00110 }; 00111 nav_status_t nav_status (void) const 00112 { 00113 return ((nav_status_t) m_data[NAV_STATUS_OFFSET]); 00114 } 00115 unsigned checksum_1 (void) const 00116 { 00117 return (m_data[CHECKSUM_1_OFFSET]); 00118 } 00119 00120 double latitude (void) const 00121 { 00122 return (letoh<f64_t> (m_data + LATITUDE_OFFSET)); 00123 } 00124 double longitude (void) const 00125 { 00126 return (letoh<f64_t> (m_data + LONGITUDE_OFFSET)); 00127 } 00128 double altitude (void) const 00129 { 00130 return (letoh<f32_t> (m_data + ALTITUDE_OFFSET)); 00131 } 00132 double n_velocity (void) const 00133 { 00134 return (1E-4 * s24 (m_data + N_VELOCITY_OFFSET)); 00135 } 00136 double e_velocity (void) const 00137 { 00138 return (1E-4 * s24 (m_data + E_VELOCITY_OFFSET)); 00139 } 00140 double d_velocity (void) const 00141 { 00142 return (1E-4 * s24 (m_data + D_VELOCITY_OFFSET)); 00143 } 00144 double heading (void) const 00145 { 00146 return (1E-6 * s24 (m_data + HEADING_OFFSET)); 00147 } 00148 double pitch (void) const 00149 { 00150 return (1E-6 * s24 (m_data + PITCH_OFFSET)); 00151 } 00152 double roll (void) const 00153 { 00154 return (1E-6 * s24 (m_data + ROLL_OFFSET)); 00155 } 00156 unsigned checksum_2 (void) const 00157 { 00158 return (m_data[CHECKSUM_2_OFFSET]); 00159 } 00160 00161 unsigned channel (void) const 00162 { 00163 return (m_data[CHANNEL_OFFSET]); 00164 } 00165 unsigned char const *channel_data (void) const 00166 { 00167 return (m_data + CHANNEL_DATA_OFFSET); 00168 } 00169 unsigned checksum_3 (void) const 00170 { 00171 return (m_data[CHECKSUM_3_OFFSET]); 00172 } 00173 }; 00174 }; 00175 00176 00177 #endif