@ -35,7 +35,7 @@ static bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y,
return false ; return false ;
} }
int get_path_length_idx ( const cereal : : ModelDataV2 : : XYZTData : : Reader & line , const float path_height ) { int get_path_length_idx ( const cereal : : XYZTData : : Reader & line , const float path_height ) {
const auto line_x = line . getX ( ) ; const auto line_x = line . getX ( ) ;
int max_idx = 0 ; int max_idx = 0 ;
for ( int i = 1 ; i < TRAJECTORY_SIZE & & line_x [ i ] < = path_height ; + + i ) { for ( int i = 1 ; i < TRAJECTORY_SIZE & & line_x [ i ] < = path_height ; + + i ) {
@ -44,7 +44,7 @@ int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line, const
return max_idx ; return max_idx ;
} }
void update_leads ( UIState * s , const cereal : : RadarState : : Reader & radar_state , const cereal : : ModelDataV2 : : XYZTData : : Reader & line ) { void update_leads ( UIState * s , const cereal : : RadarState : : Reader & radar_state , const cereal : : XYZTData : : Reader & line ) {
for ( int i = 0 ; i < 2 ; + + i ) { for ( int i = 0 ; i < 2 ; + + i ) {
auto lead_data = ( i = = 0 ) ? radar_state . getLeadOne ( ) : radar_state . getLeadTwo ( ) ; auto lead_data = ( i = = 0 ) ? radar_state . getLeadOne ( ) : radar_state . getLeadTwo ( ) ;
if ( lead_data . getStatus ( ) ) { if ( lead_data . getStatus ( ) ) {
@ -54,7 +54,7 @@ void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, con
} }
} }
void update_line_data ( const UIState * s , const cereal : : ModelDataV2 : : XYZTData : : Reader & line , void update_line_data ( const UIState * s , const cereal : : XYZTData : : Reader & line ,
float y_off , float z_off , QPolygonF * pvd , int max_idx , bool allow_invert = true ) { float y_off , float z_off , QPolygonF * pvd , int max_idx , bool allow_invert = true ) {
const auto line_x = line . getX ( ) , line_y = line . getY ( ) , line_z = line . getZ ( ) ; const auto line_x = line . getX ( ) , line_y = line . getY ( ) , line_z = line . getZ ( ) ;
QPolygonF left_points , right_points ; QPolygonF left_points , right_points ;
@ -79,10 +79,15 @@ void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Rea
* pvd = left_points + right_points ; * pvd = left_points + right_points ;
} }
void update_model ( UIState * s , const cereal : : ModelDataV2 : : Reader & model ) { void update_model ( UIState * s ,
const cereal : : ModelDataV2 : : Reader & model ,
const cereal : : UiPlan : : Reader & plan ) {
UIScene & scene = s - > scene ; UIScene & scene = s - > scene ;
auto model_position = model . getPosition ( ) ; auto plan_position = plan . getPosition ( ) ;
float max_distance = std : : clamp ( model_position . getX ( ) [ TRAJECTORY_SIZE - 1 ] , if ( plan_position . getX ( ) . size ( ) < TRAJECTORY_SIZE ) {
plan_position = model . getPosition ( ) ;
}
float max_distance = std : : clamp ( plan_position . getX ( ) [ TRAJECTORY_SIZE - 1 ] ,
MIN_DRAW_DISTANCE , MAX_DRAW_DISTANCE ) ; MIN_DRAW_DISTANCE , MAX_DRAW_DISTANCE ) ;
// update lane lines
// update lane lines
@ -108,8 +113,8 @@ void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) {
const float lead_d = lead_one . getDRel ( ) * 2. ; const float lead_d = lead_one . getDRel ( ) * 2. ;
max_distance = std : : clamp ( ( float ) ( lead_d - fmin ( lead_d * 0.35 , 10. ) ) , 0.0f , max_distance ) ; max_distance = std : : clamp ( ( float ) ( lead_d - fmin ( lead_d * 0.35 , 10. ) ) , 0.0f , max_distance ) ;
} }
max_idx = get_path_length_idx ( model _position, max_distance ) ; max_idx = get_path_length_idx ( plan _position, max_distance ) ;
update_line_data ( s , model _position, 0.9 , 1.22 , & scene . track_vertices , max_idx , false ) ; update_line_data ( s , plan _position, 0.9 , 1.22 , & scene . track_vertices , max_idx , false ) ;
} }
void update_dmonitoring ( UIState * s , const cereal : : DriverStateV2 : : Reader & driverstate , float dm_fade_state , bool is_rhd ) { void update_dmonitoring ( UIState * s , const cereal : : DriverStateV2 : : Reader & driverstate , float dm_fade_state , bool is_rhd ) {
@ -248,6 +253,7 @@ UIState::UIState(QObject *parent) : QObject(parent) {
" modelV2 " , " controlsState " , " liveCalibration " , " radarState " , " deviceState " , " roadCameraState " , " modelV2 " , " controlsState " , " liveCalibration " , " radarState " , " deviceState " , " roadCameraState " ,
" pandaStates " , " carParams " , " driverMonitoringState " , " carState " , " liveLocationKalman " , " driverStateV2 " , " pandaStates " , " carParams " , " driverMonitoringState " , " carState " , " liveLocationKalman " , " driverStateV2 " ,
" wideRoadCameraState " , " managerState " , " navInstruction " , " navRoute " , " gnssMeasurements " , " wideRoadCameraState " , " managerState " , " navInstruction " , " navRoute " , " gnssMeasurements " ,
" uiPlan " ,
} ) ; } ) ;
Params params ; Params params ;
Harald Schäfer
2 years ago
committed by
GitHub