00001 #ifndef IBEO_SCAN_MSG_H 00002 #define IBEO_SCAN_MSG_H 00003 00004 00005 // Copyright (C) 2007 University of Louisiana at Lafayette 00006 00007 00008 #include "msg.H" 00009 00010 00011 namespace ibeo 00012 { 00013 00014 class scan_msg_t : public msg_t 00015 { 00016 protected: 00017 enum { VERSION_OFFSET = 16 }; 00018 enum { SCANNER_TYPE_OFFSET = 17 }; 00019 enum { ECU_ID_OFFSET = 18 }; 00020 enum { TIMESTAMP_OFFSET = 20 }; 00021 enum { START_ANGLE_OFFSET = 24 }; 00022 enum { END_ANGLE_OFFSET = 26 }; 00023 enum { SCAN_COUNTER_OFFSET = 28 }; 00024 enum { NUM_POINTS_OFFSET = 30 }; 00025 00026 enum { POINT_OFFSET = 32 }; 00027 enum { POINT_SIZE = 12 }; 00028 00029 public: 00030 scan_msg_t () {} 00031 scan_msg_t (msg_t const &msg_) : msg_t (msg_) {} 00032 00033 unsigned version () const 00034 { return m_data[VERSION_OFFSET]; } 00035 00036 unsigned scanner_type () const 00037 { return m_data[SCANNER_TYPE_OFFSET]; } 00038 unsigned ecu_id () const 00039 { return m_data[ECU_ID_OFFSET]; } 00040 double timestamp () const 00041 { return .001 * betoh<u32_t> (m_data + TIMESTAMP_OFFSET); } 00042 double start_angle () const 00043 { return .0001 * betoh<u16_t> (m_data + START_ANGLE_OFFSET); } 00044 double end_angle () const 00045 { return .0001 * betoh<u16_t> (m_data + END_ANGLE_OFFSET); } 00046 unsigned scan_counter () const 00047 { return betoh<u16_t> (m_data + SCAN_COUNTER_OFFSET); } 00048 unsigned num_points () const 00049 { return betoh<u16_t> (m_data + NUM_POINTS_OFFSET); } 00050 00051 class point_t 00052 { 00053 protected: 00054 friend class scan_msg_t; 00055 00056 enum { SCANNER_ID_OFFSET = 0 }; 00057 enum { CHANNEL_OFFSET = 1 }; 00058 enum { SUB_CHANNEL_OFFSET = 2 }; 00059 enum { STATUS_OFFSET = 3 }; 00060 enum { X_OFFSET = 4 }; 00061 enum { Y_OFFSET = 6 }; 00062 enum { Z_OFFSET = 8 }; 00063 enum { ECHO_PULSE_WIDTH_OFFSET = 10 }; 00064 00065 unsigned char const *m_data; 00066 00067 static double dist (void const *data_) 00068 { 00069 s16_t n = betoh<s16_t> (data_); 00070 00071 if (n < -10000) 00072 return (n * 0.1) + 900; 00073 else if (n <= 10000) 00074 return n * 0.01; 00075 else 00076 return (n * 0.1) - 900; 00077 } 00078 00079 public: 00080 point_t () : m_data (0) {} 00081 00082 unsigned scanner_id () const 00083 { return m_data[SCANNER_ID_OFFSET]; } 00084 unsigned channel () const 00085 { return m_data[CHANNEL_OFFSET]; } 00086 unsigned sub_channel () const 00087 { return m_data[SUB_CHANNEL_OFFSET]; } 00088 00089 enum status_t 00090 { 00091 STATUS_OK, 00092 STATUS_INVALID, 00093 STATUS_RAIN, 00094 STATUS_GROUND, 00095 STATUS_DIRT, 00096 }; 00097 00098 status_t status () const 00099 { return (status_t) m_data[STATUS_OFFSET]; } 00100 double x () const 00101 { return dist (m_data + X_OFFSET); } 00102 double y () const 00103 { return dist (m_data + Y_OFFSET); } 00104 double z () const 00105 { return dist (m_data + Z_OFFSET); } 00106 }; 00107 00108 void point (unsigned i_, point_t &point_) const 00109 { point_.m_data = m_data + POINT_OFFSET + i_ * POINT_SIZE; } 00110 }; 00111 00112 00113 }; 00114 00115 00116 #endif