@ -110,11 +110,13 @@ static void ui_draw_circle_image(NVGcontext *vg, float x, float y, int size, int
ui_draw_circle_image ( vg , x , y , size , image , nvgRGBA ( 0 , 0 , 0 , ( 255 * bg_alpha ) ) , img_alpha ) ;
}
static void draw_lead ( UIState * s , float d_rel , float v_rel , float y_rel ) {
static void draw_lead ( UIState * s , const cereal : : RadarState : : LeadData : : Reader & lead ) {
// Draw lead car indicator
float fillAlpha = 0 ;
float speedBuff = 10. ;
float leadBuff = 40. ;
float d_rel = lead . getDRel ( ) ;
float v_rel = lead . getVRel ( ) ;
if ( d_rel < leadBuff ) {
fillAlpha = 255 * ( 1.0 - ( d_rel / leadBuff ) ) ;
if ( v_rel < 0 ) {
@ -122,7 +124,7 @@ static void draw_lead(UIState *s, float d_rel, float v_rel, float y_rel){
}
fillAlpha = ( int ) ( fmin ( fillAlpha , 255 ) ) ;
}
draw_chevron ( s , d_rel , y_rel , 25 , nvgRGBA ( 201 , 34 , 49 , fillAlpha ) , COLOR_YELLOW ) ;
draw_chevron ( s , d_rel , lead . getYRel ( ) , 25 , nvgRGBA ( 201 , 34 , 49 , fillAlpha ) , COLOR_YELLOW ) ;
}
static void ui_draw_lane_line ( UIState * s , const model_path_vertices_data * pvd , NVGcolor color ) {
@ -154,7 +156,7 @@ static void update_track_data(UIState *s, bool is_mpc, track_vertices_data *pvd)
bool started = false ;
float off = is_mpc ? 0.3 : 0.5 ;
float lead_d = scene - > lead_d_rel * 2. ;
float lead_d = scene - > lead_data [ 0 ] . getDRel ( ) * 2. ;
float path_height = is_mpc ? ( lead_d > 5. ) ? fmin ( lead_d , 25. ) - fmin ( lead_d * 0.35 , 10. ) : 20.
: ( lead_d > 0. ) ? fmin ( lead_d , 50. ) - fmin ( lead_d * 0.35 , 10. ) : 49. ;
pvd - > cnt = 0 ;
@ -205,7 +207,7 @@ static void update_all_track_data(UIState *s) {
// Draw vision path
update_track_data ( s , false , & s - > track_vertices [ 0 ] ) ;
if ( scene - > engaged ) {
if ( scene - > controls_state . getEnabled ( ) ) {
// Draw MPC path when engaged
update_track_data ( s , true , & s - > track_vertices [ 1 ] ) ;
}
@ -340,10 +342,9 @@ static void ui_draw_lane(UIState *s, const PathData *path, model_path_vertices_d
static void ui_draw_vision_lanes ( UIState * s ) {
const UIScene * scene = & s - > scene ;
model_path_vertices_data * pvd = & s - > model_path_vertices [ 0 ] ;
if ( s - > model_changed ) {
if ( s - > sm - > updated ( " model " ) ) {
update_all_lane_lines_data ( s , scene - > model . left_lane , pvd ) ;
update_all_lane_lines_data ( s , scene - > model . right_lane , pvd + MODEL_LANE_PATH_CNT ) ;
s - > model_changed = false ;
}
// Draw left lane edge
ui_draw_lane (
@ -357,13 +358,12 @@ static void ui_draw_vision_lanes(UIState *s) {
pvd + MODEL_LANE_PATH_CNT ,
nvgRGBAf ( 1.0 , 1.0 , 1.0 , scene - > model . right_lane . prob ) ) ;
if ( s - > livempc_or_radarstate_changed ) {
if ( s - > sm - > updated ( " radarState " ) ) {
update_all_track_data ( s ) ;
s - > livempc_or_radarstate_changed = false ;
}
// Draw vision path
ui_draw_track ( s , false , & s - > track_vertices [ 0 ] ) ;
if ( scene - > engaged ) {
if ( scene - > controls_state . getEnabled ( ) ) {
// Draw MPC path when engaged
ui_draw_track ( s , true , & s - > track_vertices [ 1 ] ) ;
}
@ -394,11 +394,11 @@ static void ui_draw_world(UIState *s) {
// Draw lane edges and vision/mpc tracks
ui_draw_vision_lanes ( s ) ;
if ( scene - > lead_s tatus ) {
draw_lead ( s , scene - > lead_d_rel , scene - > lead_v_rel , scene - > lead_y_rel ) ;
if ( scene - > lead_data [ 0 ] . getS tatus ( ) ) {
draw_lead ( s , scene - > lead_data [ 0 ] ) ;
}
if ( ( scene - > lead_status2 ) & & ( std : : abs ( scene - > lead_d_rel - scene - > lead_d_rel2 ) > 3.0 ) ) {
draw_lead ( s , scene - > lead_d_rel2 , scene - > lead_v_rel2 , scene - > lead_y_rel2 ) ;
if ( scene - > lead_data [ 1 ] . getStatus ( ) & & ( std : : abs ( scene - > lead_data [ 0 ] . getDRel ( ) - scene - > lead_data [ 1 ] . getDRel ( ) ) > 3.0 ) ) {
draw_lead ( s , scene - > lead_data [ 1 ] ) ;
}
nvgRestore ( s - > vg ) ;
}
@ -408,7 +408,7 @@ static void ui_draw_vision_maxspeed(UIState *s) {
return ;
} */
char maxspeed_str [ 32 ] ;
float maxspeed = s - > scene . v_cruise ;
float maxspeed = s - > scene . controls_state . getVCruise ( ) ;
int maxspeed_calc = maxspeed * 0.6225 + 0.5 ;
float speedlimit = s - > scene . speedlimit ;
int speedlim_calc = speedlimit * 2.2369363 + 0.5 ;
@ -421,7 +421,7 @@ static void ui_draw_vision_maxspeed(UIState *s) {
bool is_cruise_set = ( maxspeed ! = 0 & & maxspeed ! = SET_SPEED_NA ) ;
bool is_speedlim_valid = s - > scene . speedlimit_valid ;
bool is_set_over_limit = is_speedlim_valid & & s - > scene . engaged & &
bool is_set_over_limit = is_speedlim_valid & & s - > scene . controls_state . getEnabled ( ) & &
is_cruise_set & & maxspeed_calc > ( speedlim_calc + speed_lim_off ) ;
int viz_maxspeed_w = 184 ;
@ -476,7 +476,7 @@ static void ui_draw_vision_speedlimit(UIState *s) {
if ( s - > is_ego_over_limit ) {
hysteresis_offset = 0.0 ;
}
s - > is_ego_over_limit = is_speedlim_valid & & s - > scene . v_ego > ( speedlimit + s - > speed_lim_off + hysteresis_offset ) ;
s - > is_ego_over_limit = is_speedlim_valid & & s - > scene . controls_state . getVEgo ( ) > ( speedlimit + s - > speed_lim_off + hysteresis_offset ) ;
int viz_speedlim_w = 180 ;
int viz_speedlim_h = 202 ;
@ -522,9 +522,10 @@ static void ui_draw_vision_speedlimit(UIState *s) {
static void ui_draw_vision_speed ( UIState * s ) {
const UIScene * scene = & s - > scene ;
float speed = s - > scene . v_ego * 2.2369363 + 0.5 ;
float v_ego = s - > scene . controls_state . getVEgo ( ) ;
float speed = v_ego * 2.2369363 + 0.5 ;
if ( s - > is_metric ) {
speed = s - > scene . v_ego * 3.6 + 0.5 ;
speed = v_ego * 3.6 + 0.5 ;
}
const int viz_speed_w = 280 ;
const int viz_speed_x = scene - > ui_viz_rx + ( ( scene - > ui_viz_rw / 2 ) - ( viz_speed_w / 2 ) ) ;
@ -543,7 +544,7 @@ static void ui_draw_vision_event(UIState *s) {
const int viz_event_w = 220 ;
const int viz_event_x = ( ( s - > scene . ui_viz_rx + s - > scene . ui_viz_rw ) - ( viz_event_w + ( bdr_s * 2 ) ) ) ;
const int viz_event_y = ( box_y + ( bdr_s * 1.5 ) ) ;
if ( s - > scene . decel_for_model & & s - > scene . engaged ) {
if ( s - > scene . controls_state . getDecelForModel ( ) & & s - > scene . controls_state . getEnabled ( ) ) {
// draw winding road sign
const int img_turn_size = 160 * 1.5 ;
ui_draw_image ( s - > vg , viz_event_x - ( img_turn_size / 4 ) , viz_event_y + bdr_s - 25 , img_turn_size , img_turn_size , s - > img_turn , 1.0f ) ;
@ -561,7 +562,7 @@ static void ui_draw_vision_event(UIState *s) {
color = nvgRGBA ( 23 , 51 , 73 , 255 ) ;
}
if ( s - > scene . engageable ) {
if ( s - > scene . controls_stat e. getE ngageable( ) ) {
ui_draw_circle_image ( s - > vg , bg_wheel_x , bg_wheel_y , bg_wheel_size , s - > img_wheel , color , 1.0f , bg_wheel_y - 25 ) ;
}
}
@ -578,7 +579,7 @@ static void ui_draw_vision_face(UIState *s) {
const int face_size = 96 ;
const int face_x = ( s - > scene . ui_viz_rx + face_size + ( bdr_s * 2 ) ) ;
const int face_y = ( footer_y + ( ( footer_h - face_size ) / 2 ) ) ;
ui_draw_circle_image ( s - > vg , face_x , face_y , face_size , s - > img_face , s - > scene . monitoring_active ) ;
ui_draw_circle_image ( s - > vg , face_x , face_y , face_size , s - > img_face , s - > scene . controls_state . getDriverMonitoringOn ( ) ) ;
}
static void ui_draw_driver_view ( UIState * s ) {
@ -610,30 +611,31 @@ static void ui_draw_driver_view(UIState *s) {
ui_draw_rect ( s - > vg , valid_frame_x + valid_frame_w , box_y , frame_w - valid_frame_w - ( valid_frame_x - frame_x ) , box_h , nvgRGBA ( 23 , 51 , 73 , 255 ) ) ;
// draw face box
if ( scene - > face_prob > 0.4 ) {
if ( scene - > driver_state . getFaceProb ( ) > 0.4 ) {
auto fxy_list = scene - > driver_state . getFacePosition ( ) ;
const int face_x = fxy_list [ 0 ] ;
const int face_y = fxy_list [ 1 ] ;
int fbox_x ;
int fbox_y = box_y + ( scene - > face_y + 0.5 ) * box_h - 0.5 * 0.6 * box_h / 2 ; ;
int fbox_y = box_y + ( face_y + 0.5 ) * box_h - 0.5 * 0.6 * box_h / 2 ; ;
if ( ! scene - > is_rhd ) {
fbox_x = valid_frame_x + ( 1 - ( scene - > face_x + 0.5 ) ) * ( box_h / 2 ) - 0.5 * 0.6 * box_h / 2 ;
fbox_x = valid_frame_x + ( 1 - ( face_x + 0.5 ) ) * ( box_h / 2 ) - 0.5 * 0.6 * box_h / 2 ;
} else {
fbox_x = valid_frame_x + valid_frame_w - box_h / 2 + ( scene - > face_x + 0.5 ) * ( box_h / 2 ) - 0.5 * 0.6 * box_h / 2 ;
fbox_x = valid_frame_x + valid_frame_w - box_h / 2 + ( face_x + 0.5 ) * ( box_h / 2 ) - 0.5 * 0.6 * box_h / 2 ;
}
if ( std : : abs ( scene - > face_x ) < = 0.35 & & std : : abs ( scene - > face_y ) < = 0.4 ) {
if ( std : : abs ( face_x ) < = 0.35 & & std : : abs ( face_y ) < = 0.4 ) {
ui_draw_rect ( s - > vg , fbox_x , fbox_y , 0.6 * box_h / 2 , 0.6 * box_h / 2 ,
nvgRGBAf ( 1.0 , 1.0 , 1.0 , 0.8 - ( ( std : : abs ( scene - > face_x ) > std : : abs ( scene - > face_y ) ? std : : abs ( scene - > face_x ) : std : : abs ( scene - > face_y ) ) ) * 0.6 / 0.375 ) ,
nvgRGBAf ( 1.0 , 1.0 , 1.0 , 0.8 - ( ( std : : abs ( face_x ) > std : : abs ( face_y ) ? std : : abs ( face_x ) : std : : abs ( face_y ) ) ) * 0.6 / 0.375 ) ,
35 , 10 ) ;
} else {
ui_draw_rect ( s - > vg , fbox_x , fbox_y , 0.6 * box_h / 2 , 0.6 * box_h / 2 , nvgRGBAf ( 1.0 , 1.0 , 1.0 , 0.2 ) , 35 , 10 ) ;
}
} else {
;
}
// draw face icon
const int face_size = 85 ;
const int face_ x = ( valid_frame_x + face_size + ( bdr_s * 2 ) ) + ( scene - > is_rhd ? valid_frame_w - box_h / 2 : 0 ) ;
const int face_ y = ( box_y + box_h - face_size - bdr_s - ( bdr_s * 1.5 ) ) ;
ui_draw_circle_image ( s - > vg , face_ x, face_ y, face_size , s - > img_face , scene - > face_prob > 0.4 ) ;
const int x = ( valid_frame_x + face_size + ( bdr_s * 2 ) ) + ( scene - > is_rhd ? valid_frame_w - box_h / 2 : 0 ) ;
const int y = ( box_y + box_h - face_size - bdr_s - ( bdr_s * 1.5 ) ) ;
ui_draw_circle_image ( s - > vg , x , y , face_size , s - > img_face , scene - > driver_state . getFaceProb ( ) > 0.4 ) ;
}
static void ui_draw_vision_header ( UIState * s ) {