00001 // Copyright (C) 2008 Christopher MIRE 00002 00003 #ifndef SICK_MSG_0xF5_H 00004 #define SICK_MSG_0xF5_H 00005 00006 #include <cassert> 00007 #include <stdint.h> 00008 00009 #include "sick_msg.H" 00010 00011 namespace sick 00012 { 00013 // Measured Values 00014 class msg_0xF5_t : public msg_t 00015 { 00016 protected: 00017 static unsigned const SCAN_FLAGS_OFFSET = 7; 00018 static unsigned const BEAM_OFFSET = 13; 00019 static unsigned const BEAM_SIZE = 2; 00020 00021 public: 00022 msg_0xF5_t (); 00023 msg_0xF5_t (msg_t const *msg) : msg_t (msg) {} 00024 00025 unsigned num_ranges (void) const 00026 { 00027 return (letoh<u16_t> (m_data + 5)); 00028 } 00029 00030 unsigned scan_flags (void) const 00031 { 00032 return (letoh<u16_t> (m_data + SCAN_FLAGS_OFFSET)); 00033 } 00034 00035 unsigned num_beams (unsigned r) const 00036 { 00037 assert (r < num_ranges ()); 00038 00039 unsigned num_beams = 00040 letoh<u16_t>(m_data + SCAN_FLAGS_OFFSET + 2) - 00041 letoh<u16_t> (m_data + SCAN_FLAGS_OFFSET); 00042 00043 unsigned range_offset = SCAN_FLAGS_OFFSET; 00044 range_offset += num_beams * (BEAM_SIZE * 2) + 6; 00045 00046 for (unsigned i = 1; i < r; ++i) 00047 { 00048 num_beams = ((scan_flags () + range_offset + 2) 00049 & 0x0191) - 00050 ((scan_flags () + range_offset) 00051 & 0x0191); 00052 range_offset += num_beams * (BEAM_SIZE * 2) + 6; 00053 } 00054 00055 return num_beams; 00056 } 00057 00058 enum units_t 00059 { 00060 UNTIS_CM = 0, 00061 UNTIS_MM = 1, 00062 }; 00063 00064 units_t units (void) const 00065 { 00066 return ((units_t) (((scan_flags () + 4) >> 14) & 0x03)); 00067 } 00068 00069 uint16_t beam_dis (unsigned b) const 00070 { 00071 assert (b < num_beams (0)); 00072 return (letoh<u16_t> 00073 (m_data + BEAM_OFFSET + b * (BEAM_SIZE * 2))); 00074 } 00075 00076 uint16_t beam_reflect (unsigned b) const 00077 { 00078 assert (b < num_beams (0)); 00079 return (letoh<u16_t> 00080 (m_data + BEAM_OFFSET + BEAM_SIZE 00081 + b * (BEAM_SIZE * 2))); 00082 } 00083 00084 unsigned scan_index (void) const 00085 { 00086 unsigned offset = SCAN_FLAGS_OFFSET; 00087 for (unsigned i = 0; i < num_ranges (); ++i) 00088 offset += num_beams (i) * (BEAM_SIZE * 2) + 6; 00089 00090 return (m_data[offset]); 00091 } 00092 unsigned dgram_index (void) const 00093 { 00094 unsigned offset = SCAN_FLAGS_OFFSET; 00095 for (unsigned i = 0; i < num_ranges (); ++i) 00096 offset += num_beams (i) * (BEAM_SIZE * 2) + 6; 00097 00098 return (m_data[offset + 1]); 00099 } 00100 }; 00101 }; 00102 00103 #endif