00001 // Copyright (C) 2007 University of Louisiana at Lafayette 00002 //Authors: chandan 00003 00004 #ifndef CAJUN_ITERIS_FRAME_H 00005 #define CAJUN_ITERIS_FRAME_H 00006 00007 #include <cstdio> 00008 namespace pcan 00009 { 00010 00011 class iteris_frame_t : public pcan_frame_t 00012 { 00013 protected: 00014 00015 static unsigned const STX = 0x550; 00016 00017 static unsigned const MASK_FOR_NUM_OF_LINES = 0x03; 00018 static unsigned const NIBBLE_MASK = 0x0f; 00019 00020 static unsigned const POS_FOR_NUM_OF_LINES_ON_LEFT = 2; 00021 static unsigned const SENSOR_STATUS_BITS_POS_OFFSET = 4; 00022 static unsigned const CURVATURE_OF_LANE_POS_OFFSET = 4; 00023 00024 static unsigned const LATERAL_OFFSET_ON_LEFT = 1; 00025 static unsigned const LATERAL_OFFSET_ON_RIGHT = 2; 00026 static unsigned const ANGLE_TO_LANE_LO_OFFSET = 3; 00027 static unsigned const ANGLE_TO_LANE_HI_OFFSET = 4; 00028 static unsigned const CURVATURE_OF_LANE_LO_OFFSET = 4; 00029 static unsigned const CURVATURE_OF_LANE_HI_OFFSET = 5; 00030 static unsigned const LDW_STATUS = 6; 00031 static unsigned const LANE_DETECTION_DISTANCE = 7; 00032 00033 static unsigned const VEHICLE_SPEED_STATUS = 0x01; 00034 static unsigned const SYSTEM_AVAILABLE_STATUS = 0x08; 00035 static unsigned const WARNING_ON_LEFT = 0x02; 00036 static unsigned const WARNING_ON_RIGHT = 0x04; 00037 00038 static unsigned const TYPE_OF_LEFT_LANE_MARK = 0x01; 00039 static unsigned const VEHICLE_CROSSING_LEFT_LANE = 0x02; 00040 static unsigned const TYPE_OF_RIGHT_LANE_MARK = 0x04; 00041 static unsigned const VEHICLE_CROSSING_RIGHT_LANE = 0x08; 00042 static unsigned const LEFT_LATERAL_OFFSET_VALIDITY = 0x10; 00043 static unsigned const RIGHT_LATERAL_OFFSET_VALIDITY = 0x20; 00044 static unsigned const LANE_CURVATURE_VALIDITY = 0x40; 00045 static unsigned const LANE_ANGLE_VALIDITY = 0x80; 00046 00047 static double const LATERAL_OFFSET_RESOLUTION = 0.03125; 00048 static double const ANGLE_TO_LANE_RESOLUTION = 0.04395; 00049 static double const CURVATURE_OF_LANE_RESOLUTION = 0.00001; 00050 00051 public: 00052 iteris_frame_t (pcan_frame_t const &frame_) : pcan_frame_t (frame_) {} 00053 00054 bool vehicle_speed_status () const 00055 { 00056 unsigned temp = (m_msg->DATA[0] >> SENSOR_STATUS_BITS_POS_OFFSET); 00057 return (temp & (VEHICLE_SPEED_STATUS))== VEHICLE_SPEED_STATUS; 00058 } 00059 00060 bool system_status () const 00061 { 00062 unsigned temp = (m_msg->DATA[0] >> SENSOR_STATUS_BITS_POS_OFFSET); 00063 return (temp & (SYSTEM_AVAILABLE_STATUS))== SYSTEM_AVAILABLE_STATUS; 00064 } 00065 00066 bool warning_on_left () const 00067 { 00068 unsigned temp = (m_msg->DATA[0] >> SENSOR_STATUS_BITS_POS_OFFSET); 00069 return (temp & (WARNING_ON_LEFT))== WARNING_ON_LEFT; 00070 } 00071 00072 bool warning_on_right () const 00073 { 00074 unsigned temp = (m_msg->DATA[0] >> SENSOR_STATUS_BITS_POS_OFFSET); 00075 return (temp & (WARNING_ON_RIGHT))== WARNING_ON_RIGHT; 00076 } 00077 00078 bool type_of_lanemark_on_left () const 00079 { 00080 return (m_msg->DATA[LDW_STATUS] & TYPE_OF_LEFT_LANE_MARK) == TYPE_OF_LEFT_LANE_MARK; 00081 } 00082 00083 bool type_of_lanemark_on_right () const 00084 { 00085 return (m_msg->DATA[LDW_STATUS] & TYPE_OF_RIGHT_LANE_MARK) == TYPE_OF_RIGHT_LANE_MARK; 00086 } 00087 00088 bool vehicle_crossing_left_lane () const 00089 { 00090 return (m_msg->DATA[LDW_STATUS] & VEHICLE_CROSSING_LEFT_LANE) == VEHICLE_CROSSING_LEFT_LANE; 00091 } 00092 00093 bool vehicle_crossing_right_lane () const 00094 { 00095 return (m_msg->DATA[LDW_STATUS] & VEHICLE_CROSSING_RIGHT_LANE) == VEHICLE_CROSSING_RIGHT_LANE; 00096 } 00097 00098 bool left_lateral_offset_validity () const 00099 { 00100 return (m_msg->DATA[LDW_STATUS] & LEFT_LATERAL_OFFSET_VALIDITY) == LEFT_LATERAL_OFFSET_VALIDITY; 00101 } 00102 00103 bool right_lateral_offset_validity () const 00104 { 00105 return (m_msg->DATA[LDW_STATUS] & RIGHT_LATERAL_OFFSET_VALIDITY) == RIGHT_LATERAL_OFFSET_VALIDITY; 00106 } 00107 00108 bool lane_curvature_validity () const 00109 { 00110 return (m_msg->DATA[LDW_STATUS] & LANE_CURVATURE_VALIDITY) == LANE_CURVATURE_VALIDITY; 00111 } 00112 00113 bool lane_angle_validity () const 00114 { 00115 return (m_msg->DATA[LDW_STATUS] & LANE_ANGLE_VALIDITY) == LANE_ANGLE_VALIDITY; 00116 } 00117 00118 00119 unsigned num_of_lines_on_right () const 00120 { 00121 return (m_msg->DATA[0] & MASK_FOR_NUM_OF_LINES); 00122 } 00123 00124 unsigned num_of_lines_on_left () const 00125 { 00126 return ((m_msg->DATA[0] >> POS_FOR_NUM_OF_LINES_ON_LEFT) & 00127 MASK_FOR_NUM_OF_LINES); 00128 } 00129 00130 double lateral_offset_to_left_lane () const 00131 { 00132 return (LATERAL_OFFSET_RESOLUTION * m_msg->DATA[LATERAL_OFFSET_ON_LEFT]); 00133 } 00134 00135 double lateral_offset_to_right_lane () const 00136 { 00137 return (LATERAL_OFFSET_RESOLUTION * m_msg->DATA[LATERAL_OFFSET_ON_RIGHT]); 00138 } 00139 00140 double bot_angle_wrt_lane () const 00141 { 00142 int value = 0x00; 00143 int temp = (m_msg->DATA[ANGLE_TO_LANE_HI_OFFSET] & 0x08); 00144 if (temp != 0x08) 00145 { 00146 value = (m_msg->DATA[ANGLE_TO_LANE_HI_OFFSET] & NIBBLE_MASK) & 0x07; 00147 return ( ANGLE_TO_LANE_RESOLUTION * (( value << 8) | 00148 (m_msg->DATA[ANGLE_TO_LANE_LO_OFFSET]))); 00149 } 00150 else 00151 { 00152 value = (((m_msg->DATA[ANGLE_TO_LANE_HI_OFFSET] & NIBBLE_MASK) << 8) | 00153 (m_msg->DATA[ANGLE_TO_LANE_LO_OFFSET])); 00154 value = (value) ^ 0x0fff; 00155 return (-1 * ANGLE_TO_LANE_RESOLUTION * (value + 1)); 00156 } 00157 00158 } 00159 00160 double lane_curvature () const 00161 { 00162 int value = 0x00; 00163 int negative_value = 0x00; 00164 unsigned temp = (m_msg->DATA[CURVATURE_OF_LANE_HI_OFFSET] & 0x80); 00165 if (temp != 0x80) 00166 { 00167 value = m_msg->DATA[CURVATURE_OF_LANE_HI_OFFSET] & 0x7f; 00168 return (CURVATURE_OF_LANE_RESOLUTION * ((value << 4) | 00169 (m_msg->DATA[CURVATURE_OF_LANE_LO_OFFSET] >> CURVATURE_OF_LANE_POS_OFFSET))); 00170 } 00171 else 00172 { 00173 value = m_msg->DATA[CURVATURE_OF_LANE_HI_OFFSET]; 00174 negative_value = ((value << 4) | 00175 (m_msg->DATA[CURVATURE_OF_LANE_LO_OFFSET] >> CURVATURE_OF_LANE_POS_OFFSET)); 00176 negative_value = (negative_value) ^ 0x0fff; 00177 return (-1 * CURVATURE_OF_LANE_RESOLUTION * (negative_value + 1)); 00178 } 00179 } 00180 00181 unsigned lane_detection_dist () const 00182 { 00183 return (m_msg->DATA[LANE_DETECTION_DISTANCE]); 00184 } 00185 }; 00186 00187 00188 }; 00189 00190 00191 #endif
1.6.1