diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index 13b6a61..48a8628 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -463,6 +463,8 @@ class VCruiseCarrot: self._pause_auto_speed_up = False if self._soft_hold_active > 0: self._soft_hold_active = 0 + elif self._cruise_ready: + pass elif self._v_cruise_kph_at_brake > 0 and v_cruise_kph < self._v_cruise_kph_at_brake: v_cruise_kph = self._v_cruise_kph_at_brake self._v_cruise_kph_at_brake = 0 @@ -476,6 +478,8 @@ class VCruiseCarrot: self._pause_auto_speed_up = True if self._soft_hold_active > 0: self._cruise_control(-1, -1, "Cruise off,softhold mode (decelCruise)") + elif self._cruise_ready: + pass elif self.v_ego_kph_set > v_cruise_kph + 2: v_cruise_kph = max(self.v_ego_kph_set, self._cruise_speed_min) elif self._cruise_button_mode in [0, 1]: diff --git a/selfdrive/carrot_settings.json b/selfdrive/carrot_settings.json index ab097de..3787aa3 100644 --- a/selfdrive/carrot_settings.json +++ b/selfdrive/carrot_settings.json @@ -212,14 +212,14 @@ { "group": "조향튜닝", "name": "SteerActuatorDelay", - "title": "SteerActuatorDelay(40)", + "title": "SteerActuatorDelay(30)", "descr": "조향지연값", "egroup": "LAT", - "etitle": "SteerActuatorDelay(40)", + "etitle": "SteerActuatorDelay(30)", "edescr": "", "min": 1, "max": 300, - "default": 40, + "default": 30, "unit": 1 }, { @@ -368,23 +368,23 @@ { "group": "주행튜닝", "name": "ComfortBrake", - "title": "Comfort Brake(240)", + "title": "Comfort Brake(250)", "descr": "x0.01, 값이 크면 급하게 브레이킹", "egroup": "LONG", - "etitle": "Comfort Brake(240)", + "etitle": "Comfort Brake(250)", "edescr": "", "min": 200, "max": 300, - "default": 240, + "default": 250, "unit": 5 }, { "group": "주행튜닝", "name": "JLeadFactor2", - "title": "JerkLeadFactor(0)", + "title": "JerkLeadFactor(100)", "descr": "값이 작으면 전방차량의 Jerk에 민감하게 반응", "egroup": "LONG", - "etitle": "JerkLeadFactor(0)", + "etitle": "JerkLeadFactor(100)", "edescr": "", "min": 10, "max": 100, @@ -394,11 +394,11 @@ { "group": "주행튜닝", "name": "LongTuningKpV", - "title": "Long KpV(100)x0.01", - "descr": "", + "title": "Long KpV(0)x0.01", + "descr": "HKG: AccelPid:0, VelocityPid:100", "egroup": "LONG", - "etitle": "Long KpV(100)x0.01", - "edescr": "", + "etitle": "Long KpV(0)x0.01", + "edescr": "HKG: AccelPid:0, VelocityPid:100", "min": 0, "max": 200, "default": 0, diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 0cbd606..cbc84f7 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -54,6 +54,9 @@ def clamp(val, min_val, max_val): clamped_val = float(np.clip(val, min_val, max_val)) return clamped_val, clamped_val != val +def smooth_value(val, prev_val, tau, dt=DT_MDL): + alpha = 1 - np.exp(-dt/tau) if tau > 0 else 1 + return alpha * val + (1 - alpha) * prev_val def clip_curvature(v_ego, prev_curvature, new_curvature, roll): # This function respects ISO lateral jerk and acceleration limits + a max curvature diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 2f2edca..fd469e5 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -26,7 +26,7 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS from openpilot.common.transformations.model import get_warp_matrix from openpilot.system import sentry from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper -from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan +from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value from openpilot.selfdrive.modeld.parse_model_outputs import Parser from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState from openpilot.selfdrive.modeld.constants import ModelConstants, Plan @@ -46,10 +46,6 @@ POLICY_METADATA_PATH = Path(__file__).parent / 'models/driving_policy_metadata.p LAT_SMOOTH_SECONDS = 0.13 LONG_SMOOTH_SECONDS = 0.3 -def smooth_value(val, prev_val, tau): - alpha = 1 - np.exp(-DT_MDL / tau) if tau > 0 else 1 - return alpha * val + (1 - alpha) * prev_val - def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, lat_action_t: float, long_action_t: float,) -> log.ModelDataV2.Action: diff --git a/selfdrive/ui/carrot.cc b/selfdrive/ui/carrot.cc index 5d4a298..549a56a 100644 --- a/selfdrive/ui/carrot.cc +++ b/selfdrive/ui/carrot.cc @@ -310,6 +310,9 @@ protected: } } + Params params; + std::deque minDeque[3]; // 최소값을 유지하는 덱 + std::deque maxDeque[3]; // 최대값을 유지하는 덱 void makePlotData(const UIState* s, float data[], char* title) { SubMaster& sm = *(s->sm); @@ -395,14 +398,14 @@ protected: plotIndex = 0; plotMin = 0.; plotMax = 0.; + for (int i = 0; i < 3; i++) { + minDeque[i].clear(); + maxDeque[i].clear(); + } show_plot_mode_prev = show_plot_mode; } } - Params params; - std::deque minDeque[3]; // 최소값을 유지하는 덱 - std::deque maxDeque[3]; // 최대값을 유지하는 덱 - void updatePlotQueue(float plot_data[3]) { // plotIndex 업데이트 plotIndex = (plotIndex + 1) % PLOT_MAX; diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 484feb5..cde0998 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -664,8 +664,8 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) { latLongToggles->addItem(new CValueControl("StoppingAccel", "LONG: StoppingStartAccelx0.01(-40)", "", "../assets/offroad/icon_logic.png", -100, 0, 5)); latLongToggles->addItem(new CValueControl("StopDistanceCarrot", "LONG: StopDistance (600)cm", "", "../assets/offroad/icon_logic.png", 300, 1000, 10)); //latLongToggles->addItem(new CValueControl("TraffStopDistanceAdjust", "LONG: TrafficStopDistance adjust(150)cm", "", "../assets/offroad/icon_road.png", -1000, 1000, 10)); - latLongToggles->addItem(new CValueControl("ComfortBrake", "LONG: Comfort Brake (240)", "x0.01", "../assets/offroad/icon_logic.png", 200, 300, 1)); - latLongToggles->addItem(new CValueControl("JLeadFactor2", "LONG: Jerk Lead Factor (0)", "x0.01", "../assets/offroad/icon_logic.png", 10, 100, 5)); + latLongToggles->addItem(new CValueControl("ComfortBrake", "LONG: Comfort Brake (250)", "x0.01", "../assets/offroad/icon_logic.png", 200, 300, 1)); + latLongToggles->addItem(new CValueControl("JLeadFactor2", "LONG: Jerk Lead Factor (100)", "x0.01", "../assets/offroad/icon_logic.png", 10, 100, 5)); latLongToggles->addItem(new CValueControl("CruiseMaxVals1", "ACCEL:0km/h(160)", "속도별 가속도를 지정합니다.(x0.01m/s^2)", "../assets/offroad/icon_logic.png", 1, 250, 5)); latLongToggles->addItem(new CValueControl("CruiseMaxVals2", "ACCEL:40km/h(120)", "속도별 가속도를 지정합니다.(x0.01m/s^2)", "../assets/offroad/icon_logic.png", 1, 250, 5)); latLongToggles->addItem(new CValueControl("CruiseMaxVals3", "ACCEL:60km/h(100)", "속도별 가속도를 지정합니다.(x0.01m/s^2)", "../assets/offroad/icon_logic.png", 1, 250, 5)); @@ -674,7 +674,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) { latLongToggles->addItem(new CValueControl("CruiseMaxVals6", "ACCEL:140km/h(60)", "속도별 가속도를 지정합니다.(x0.01m/s^2)", "../assets/offroad/icon_logic.png", 1, 250, 5)); //latLongToggles->addItem(new CValueControl("CruiseMinVals", "DECEL:(120)", "감속도를 설정합니다.(x0.01m/s^2)", "../assets/offroad/icon_road.png", 50, 250, 5)); latLongToggles->addItem(new CValueControl("MaxAngleFrames", "MaxAngleFrames(89)", "89:기본, 스티어계기판에러시 85~87", "../assets/offroad/icon_logic.png", 80, 100, 1)); - latLongToggles->addItem(new CValueControl("SteerActuatorDelay", "LAT:SteerActuatorDelay(40)", "표준", "../assets/offroad/icon_logic.png", 1, 100, 1)); + latLongToggles->addItem(new CValueControl("SteerActuatorDelay", "LAT:SteerActuatorDelay(30)", "표준", "../assets/offroad/icon_logic.png", 1, 100, 1)); latLongToggles->addItem(new CValueControl("LateralTorqueCustom", "LAT: TorqueCustom(0)", "", "../assets/offroad/icon_logic.png", 0, 2, 1)); latLongToggles->addItem(new CValueControl("LateralTorqueAccelFactor", "LAT: TorqueAccelFactor(2500)", "", "../assets/offroad/icon_logic.png", 1000, 6000, 10)); latLongToggles->addItem(new CValueControl("LateralTorqueFriction", "LAT: TorqueFriction(100)", "", "../assets/offroad/icon_logic.png", 0, 1000, 10)); diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index 1f6ad9b..f6c2681 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -26,6 +26,9 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, SpectraCamera * LOGD("allocated %d CL buffers", frame_buf_count); } + out_img_width = sensor->frame_width; + out_img_height = sensor->hdr_offset > 0 ? (sensor->frame_height - sensor->hdr_offset) / 2 : sensor->frame_height; + // the encoder HW tells us the size it wants after setting it up. // TODO: VENUS_BUFFER_SIZE should give the size, but it's too small. dependent on encoder settings? size_t nv12_size = (out_img_width <= 1344 ? 2900 : 2346)*cam->stride; diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h index c26859c..c042667 100644 --- a/system/camerad/cameras/camera_common.h +++ b/system/camerad/cameras/camera_common.h @@ -32,7 +32,7 @@ public: VisionBuf *cur_yuv_buf; VisionBuf *cur_camera_buf; std::unique_ptr camera_bufs_raw; - uint32_t out_img_width, out_img_height; + int out_img_width, out_img_height; CameraBuf() = default; ~CameraBuf(); diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 8c4602b..18a42f0 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -73,7 +73,7 @@ void CameraState::init(VisionIpcServer *v, cl_device_id device_id, cl_context ct if (!camera.enabled) return; - fl_pix = camera.cc.focal_len / camera.sensor->pixel_size_mm / camera.sensor->out_scale; + fl_pix = camera.cc.focal_len / camera.sensor->pixel_size_mm; set_exposure_rect(); dc_gain_weight = camera.sensor->dc_gain_min_weight; @@ -107,10 +107,10 @@ void CameraState::set_exposure_rect() { float fl_ref = ae_target.second; ae_xywh = (Rect){ - std::max(0, (int)camera.buf.out_img_width / 2 - (int)(fl_pix / fl_ref * xywh_ref.w / 2)), - std::max(0, (int)camera.buf.out_img_height / 2 - (int)(fl_pix / fl_ref * (h_ref / 2 - xywh_ref.y))), - std::min((int)(fl_pix / fl_ref * xywh_ref.w), (int)camera.buf.out_img_width / 2 + (int)(fl_pix / fl_ref * xywh_ref.w / 2)), - std::min((int)(fl_pix / fl_ref * xywh_ref.h), (int)camera.buf.out_img_height / 2 + (int)(fl_pix / fl_ref * (h_ref / 2 - xywh_ref.y))) + std::max(0, camera.buf.out_img_width / 2 - (int)(fl_pix / fl_ref * xywh_ref.w / 2)), + std::max(0, camera.buf.out_img_height / 2 - (int)(fl_pix / fl_ref * (h_ref / 2 - xywh_ref.y))), + std::min((int)(fl_pix / fl_ref * xywh_ref.w), camera.buf.out_img_width / 2 + (int)(fl_pix / fl_ref * xywh_ref.w / 2)), + std::min((int)(fl_pix / fl_ref * xywh_ref.h), camera.buf.out_img_height / 2 + (int)(fl_pix / fl_ref * (h_ref / 2 - xywh_ref.y))) }; } diff --git a/system/camerad/cameras/ife.h b/system/camerad/cameras/ife.h index fd87d2b..49737f2 100644 --- a/system/camerad/cameras/ife.h +++ b/system/camerad/cameras/ife.h @@ -105,7 +105,7 @@ int build_update(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std: } -int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector &patches, uint32_t out_width, uint32_t out_height) { +int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector &patches) { uint8_t *start = dst; // start with the every frame config @@ -185,12 +185,12 @@ int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo // output size/scaling dst += write_cont(dst, 0xa3c, { 0x00000003, - ((out_width - 1) << 16) | (s->frame_width - 1), + ((s->frame_width - 1) << 16) | (s->frame_width - 1), 0x30036666, 0x00000000, 0x00000000, s->frame_width - 1, - ((out_height - 1) << 16) | (s->frame_height - 1), + ((s->frame_height - 1) << 16) | (s->frame_height - 1), 0x30036666, 0x00000000, 0x00000000, @@ -198,12 +198,12 @@ int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo }); dst += write_cont(dst, 0xa68, { 0x00000003, - ((out_width / 2 - 1) << 16) | (s->frame_width - 1), + ((s->frame_width/2 - 1) << 16) | (s->frame_width - 1), 0x3006cccc, 0x00000000, 0x00000000, s->frame_width - 1, - ((out_height / 2 - 1) << 16) | (s->frame_height - 1), + ((s->frame_height/2 - 1) << 16) | (s->frame_height - 1), 0x3006cccc, 0x00000000, 0x00000000, @@ -212,12 +212,12 @@ int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo // cropping dst += write_cont(dst, 0xe10, { - out_height - 1, - out_width - 1, + s->frame_height - 1, + s->frame_width - 1, }); dst += write_cont(dst, 0xe30, { - out_height / 2 - 1, - out_width - 1, + s->frame_height/2 - 1, + s->frame_width - 1, }); dst += write_cont(dst, 0xe18, { 0x0ff00000, diff --git a/system/camerad/cameras/spectra.cc b/system/camerad/cameras/spectra.cc index 47ae906..4535078 100644 --- a/system/camerad/cameras/spectra.cc +++ b/system/camerad/cameras/spectra.cc @@ -281,21 +281,18 @@ void SpectraCamera::camera_open(VisionIpcServer *v, cl_device_id device_id, cl_c if (!enabled) return; - buf.out_img_width = sensor->frame_width / sensor->out_scale; - buf.out_img_height = (sensor->hdr_offset > 0 ? (sensor->frame_height - sensor->hdr_offset) / 2 : sensor->frame_height) / sensor->out_scale; - // size is driven by all the HW that handles frames, // the video encoder has certain alignment requirements in this case - stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, buf.out_img_width); - y_height = VENUS_Y_SCANLINES(COLOR_FMT_NV12, buf.out_img_height); - uv_height = VENUS_UV_SCANLINES(COLOR_FMT_NV12, buf.out_img_height); + stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, sensor->frame_width); + y_height = VENUS_Y_SCANLINES(COLOR_FMT_NV12, sensor->frame_height); + uv_height = VENUS_UV_SCANLINES(COLOR_FMT_NV12, sensor->frame_height); uv_offset = stride*y_height; yuv_size = uv_offset + stride*uv_height; if (cc.output_type != ISP_RAW_OUTPUT) { uv_offset = ALIGNED_SIZE(uv_offset, 0x1000); yuv_size = uv_offset + ALIGNED_SIZE(stride*uv_height, 0x1000); } - assert(stride == VENUS_UV_STRIDE(COLOR_FMT_NV12, buf.out_img_width)); + assert(stride == VENUS_UV_STRIDE(COLOR_FMT_NV12, sensor->frame_width)); assert(y_height/2 == uv_height); open = true; @@ -648,14 +645,14 @@ void SpectraCamera::config_bps(int idx, int request_id) { io_cfg[1].mem_handle[0] = buf_handle_yuv[idx]; io_cfg[1].mem_handle[1] = buf_handle_yuv[idx]; io_cfg[1].planes[0] = (struct cam_plane_cfg){ - .width = buf.out_img_width, - .height = buf.out_img_height, + .width = sensor->frame_width, + .height = sensor->frame_height, .plane_stride = stride, .slice_height = y_height, }; io_cfg[1].planes[1] = (struct cam_plane_cfg){ - .width = buf.out_img_width, - .height = buf.out_img_height / 2, + .width = sensor->frame_width, + .height = sensor->frame_height/2, .plane_stride = stride, .slice_height = uv_height, }; @@ -740,7 +737,7 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) { bool is_raw = cc.output_type != ISP_IFE_PROCESSED; if (!is_raw) { if (init) { - buf_desc[0].length = build_initial_config((unsigned char*)ife_cmd.ptr + buf_desc[0].offset, cc, sensor.get(), patches, buf.out_img_width, buf.out_img_height); + buf_desc[0].length = build_initial_config((unsigned char*)ife_cmd.ptr + buf_desc[0].offset, cc, sensor.get(), patches); } else { buf_desc[0].length = build_update((unsigned char*)ife_cmd.ptr + buf_desc[0].offset, cc, sensor.get(), patches); } @@ -847,14 +844,14 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) { io_cfg[0].mem_handle[0] = buf_handle_yuv[idx]; io_cfg[0].mem_handle[1] = buf_handle_yuv[idx]; io_cfg[0].planes[0] = (struct cam_plane_cfg){ - .width = buf.out_img_width, - .height = buf.out_img_height, + .width = sensor->frame_width, + .height = sensor->frame_height, .plane_stride = stride, .slice_height = y_height, }; io_cfg[0].planes[1] = (struct cam_plane_cfg){ - .width = buf.out_img_width, - .height = buf.out_img_height / 2, + .width = sensor->frame_width, + .height = sensor->frame_height/2, .plane_stride = stride, .slice_height = uv_height, }; @@ -996,9 +993,6 @@ bool SpectraCamera::openSensor() { LOGD("-- Probing sensor %d", cc.camera_num); auto init_sensor_lambda = [this](SensorInfo *s) { - if (s->image_sensor == cereal::FrameData::ImageSensor::OS04C10 && cc.output_type == ISP_IFE_PROCESSED) { - ((OS04C10*)s)->ife_downscale_configure(); - } sensor.reset(s); return (sensors_init() == 0); }; @@ -1071,8 +1065,8 @@ void SpectraCamera::configISP() { .data[0] = (struct cam_isp_out_port_info){ .res_type = CAM_ISP_IFE_OUT_RES_FULL, .format = CAM_FORMAT_NV12, - .width = buf.out_img_width, - .height = buf.out_img_height + sensor->extra_height, + .width = sensor->frame_width, + .height = sensor->frame_height + sensor->extra_height, .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0, }, }; @@ -1147,8 +1141,8 @@ void SpectraCamera::configICP() { }, .out_res[0] = (struct cam_icp_res_info){ .format = 0x3, // YUV420NV12 - .width = buf.out_img_width, - .height = buf.out_img_height, + .width = sensor->frame_width, + .height = sensor->frame_height, .fps = 20, }, }; diff --git a/system/camerad/sensors/os04c10.cc b/system/camerad/sensors/os04c10.cc index 38be4ec..d008e1d 100644 --- a/system/camerad/sensors/os04c10.cc +++ b/system/camerad/sensors/os04c10.cc @@ -20,17 +20,6 @@ const uint32_t os04c10_analog_gains_reg[] = { } // namespace -void OS04C10::ife_downscale_configure() { - out_scale = 2; - - pixel_size_mm = 0.002; - frame_width = 2688; - frame_height = 1520; - exposure_time_max = 2352; - - init_reg_array.insert(init_reg_array.end(), std::begin(ife_downscale_override_array_os04c10), std::end(ife_downscale_override_array_os04c10)); -} - OS04C10::OS04C10() { image_sensor = cereal::FrameData::ImageSensor::OS04C10; bayer_pattern = CAM_ISP_PATTERN_BAYER_BGBGBG; diff --git a/system/camerad/sensors/os04c10_registers.h b/system/camerad/sensors/os04c10_registers.h index b501656..8b1c78c 100644 --- a/system/camerad/sensors/os04c10_registers.h +++ b/system/camerad/sensors/os04c10_registers.h @@ -88,6 +88,8 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x37c7, 0xa8}, {0x37da, 0x11}, {0x381f, 0x08}, + // {0x3829, 0x03}, + // {0x3832, 0x00}, {0x3881, 0x00}, {0x3888, 0x04}, {0x388b, 0x00}, @@ -197,6 +199,7 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x370b, 0x48}, {0x370c, 0x01}, {0x370f, 0x00}, + {0x3714, 0x28}, {0x3716, 0x04}, {0x3719, 0x11}, {0x371a, 0x1e}, @@ -228,6 +231,7 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x37bd, 0x01}, {0x37bf, 0x26}, {0x37c0, 0x11}, + {0x37c2, 0x14}, {0x37cd, 0x19}, {0x37e0, 0x08}, {0x37e6, 0x04}, @@ -237,9 +241,14 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x37d8, 0x02}, {0x37e2, 0x10}, {0x3739, 0x10}, + {0x3662, 0x08}, {0x37e4, 0x20}, {0x37e3, 0x08}, + {0x37d9, 0x04}, {0x4040, 0x00}, + {0x4041, 0x03}, + {0x4008, 0x01}, + {0x4009, 0x06}, // FSIN {0x3002, 0x22}, @@ -260,11 +269,20 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x3802, 0x00}, {0x3803, 0x00}, {0x3804, 0x0a}, {0x3805, 0x8f}, {0x3806, 0x05}, {0x3807, 0xff}, + {0x3808, 0x05}, {0x3809, 0x40}, + {0x380a, 0x02}, {0x380b, 0xf8}, {0x3811, 0x08}, {0x3813, 0x08}, + {0x3814, 0x03}, {0x3815, 0x01}, + {0x3816, 0x03}, {0x3817, 0x01}, + {0x380c, 0x0b}, {0x380d, 0xac}, // HTS + {0x380e, 0x06}, {0x380f, 0x9c}, // VTS + + {0x3820, 0xb3}, + {0x3821, 0x01}, {0x3880, 0x00}, {0x3882, 0x20}, {0x3c91, 0x0b}, @@ -313,40 +331,4 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { // r {0x5104, 0x08}, {0x5105, 0xd6}, {0x5144, 0x08}, {0x5145, 0xd6}, - - {0x3714, 0x28}, - {0x37c2, 0x14}, - {0x3662, 0x08}, - {0x37d9, 0x04}, - {0x4041, 0x03}, - {0x4008, 0x01}, - {0x4009, 0x06}, - {0x3808, 0x05}, {0x3809, 0x40}, - {0x380a, 0x02}, {0x380b, 0xf8}, - {0x3814, 0x03}, - {0x3816, 0x03}, - {0x380c, 0x0b}, {0x380d, 0xac}, // HTS - {0x380e, 0x06}, {0x380f, 0x9c}, // VTS - {0x3820, 0xb3}, - {0x3821, 0x01}, -}; - -const struct i2c_random_wr_payload ife_downscale_override_array_os04c10[] = { - // OS04C10_AA_00_02_17_wAO_2688x1524_MIPI728Mbps_Linear12bit_20FPS_4Lane_MCLK24MHz - {0x3829, 0x03}, - {0x3714, 0x24}, - {0x37c2, 0x04}, - {0x3662, 0x10}, - {0x37d9, 0x08}, - {0x4041, 0x07}, - {0x4008, 0x02}, - {0x4009, 0x0d}, - {0x3808, 0x0a}, {0x3809, 0x80}, - {0x380a, 0x05}, {0x380b, 0xf0}, - {0x3814, 0x01}, - {0x3816, 0x01}, - {0x380c, 0x08}, {0x380d, 0x5c}, // HTS - {0x380e, 0x09}, {0x380f, 0x38}, // VTS - {0x3820, 0xb0}, - {0x3821, 0x00}, }; diff --git a/system/camerad/sensors/sensor.h b/system/camerad/sensors/sensor.h index d4be3cf..c1131aa 100644 --- a/system/camerad/sensors/sensor.h +++ b/system/camerad/sensors/sensor.h @@ -29,7 +29,6 @@ public: uint32_t frame_stride; uint32_t frame_offset = 0; uint32_t extra_height = 0; - int out_scale = 1; int registers_offset = -1; int stats_offset = -1; int hdr_offset = -1; @@ -110,7 +109,6 @@ public: class OS04C10 : public SensorInfo { public: OS04C10(); - void ife_downscale_configure(); std::vector getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override; float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override; int getSlaveAddress(int port) const override; diff --git a/system/manager/manager.py b/system/manager/manager.py index 40b0119..4bbf155 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -83,7 +83,7 @@ def get_default_params(): ("MapTurnSpeedFactor", "90"), ("StoppingAccel", "0"), ("StopDistanceCarrot", "550"), - ("ComfortBrake", "240"), + ("ComfortBrake", "250"), ("JLeadFactor2", "100"), ("CruiseButtonMode", "2"), ("CruiseButtonTest1", "8"), @@ -100,7 +100,7 @@ def get_default_params(): ("CruiseMaxVals4", "110"), ("CruiseMaxVals5", "95"), ("CruiseMaxVals6", "80"), - ("LongTuningKpV", "100"), + ("LongTuningKpV", "0"), ("LongTuningKiV", "0"), ("LongTuningKf", "100"), ("LongActuatorDelay", "20"), diff --git a/system/ui/spinner.py b/system/ui/spinner.py index 951c6f6..adfb604 100755 --- a/system/ui/spinner.py +++ b/system/ui/spinner.py @@ -9,7 +9,7 @@ from openpilot.system.ui.lib.application import gui_app # Constants PROGRESS_BAR_WIDTH = 1000 PROGRESS_BAR_HEIGHT = 20 -ROTATION_TIME_SECONDS = 1.0 # Time for one full circle +DEGREES_PER_SECOND = 360.0 # one full rotation per second MARGIN = 200 TEXTURE_SIZE = 360 FONT_SIZE = 80 @@ -26,21 +26,25 @@ class Spinner: self._spinner_texture = gui_app.load_texture_from_image(os.path.join(BASEDIR, "selfdrive/assets/img_spinner_track.png"), TEXTURE_SIZE, TEXTURE_SIZE) self._rotation = 0.0 self._text: str = "" + self._progress: int | None = None self._lock = threading.Lock() def set_text(self, text: str) -> None: with self._lock: - self._text = text + if text.isdigit(): + self._progress = clamp(int(text), 0, 100) + self._text = "" + else: + self._progress = None + self._text = text def render(self): center = rl.Vector2(gui_app.width / 2.0, gui_app.height / 2.0) spinner_origin = rl.Vector2(TEXTURE_SIZE / 2.0, TEXTURE_SIZE / 2.0) comma_position = rl.Vector2(center.x - TEXTURE_SIZE / 2.0, center.y - TEXTURE_SIZE / 2.0) - fps = rl.get_fps() - if fps > 0: - degrees_per_frame = 360.0 / (ROTATION_TIME_SECONDS * fps) - self._rotation = (self._rotation + degrees_per_frame) % 360.0 + delta_time = rl.get_frame_time() + self._rotation = (self._rotation + DEGREES_PER_SECOND * delta_time) % 360.0 # Draw rotating spinner and static comma logo rl.draw_texture_pro(self._spinner_texture, rl.Rectangle(0, 0, TEXTURE_SIZE, TEXTURE_SIZE), @@ -49,23 +53,21 @@ class Spinner: rl.draw_texture_v(self._comma_texture, comma_position, rl.WHITE) # Display progress bar or text based on user input - text = None + y_pos = rl.get_screen_height() - MARGIN - PROGRESS_BAR_HEIGHT with self._lock: + progress = self._progress text = self._text - if text: - y_pos = rl.get_screen_height() - MARGIN - PROGRESS_BAR_HEIGHT - if text.isdigit(): - progress = clamp(int(text), 0, 100) - bar = rl.Rectangle(center.x - PROGRESS_BAR_WIDTH / 2.0, y_pos, PROGRESS_BAR_WIDTH, PROGRESS_BAR_HEIGHT) - rl.draw_rectangle_rounded(bar, 1, 10, DARKGRAY) + if progress is not None: + bar = rl.Rectangle(center.x - PROGRESS_BAR_WIDTH / 2.0, y_pos, PROGRESS_BAR_WIDTH, PROGRESS_BAR_HEIGHT) + rl.draw_rectangle_rounded(bar, 1, 10, DARKGRAY) - bar.width *= progress / 100.0 - rl.draw_rectangle_rounded(bar, 1, 10, rl.WHITE) - else: - text_size = rl.measure_text_ex(gui_app.font(), text, FONT_SIZE, 1.0) - rl.draw_text_ex(gui_app.font(), text, - rl.Vector2(center.x - text_size.x / 2, y_pos), FONT_SIZE, 1.0, rl.WHITE) + bar.width *= progress / 100.0 + rl.draw_rectangle_rounded(bar, 1, 10, rl.WHITE) + elif text: + text_size = rl.measure_text_ex(gui_app.font(), text, FONT_SIZE, 1.0) + rl.draw_text_ex(gui_app.font(), text, + rl.Vector2(center.x - text_size.x / 2, y_pos), FONT_SIZE, 1.0, rl.WHITE) if __name__ == "__main__": diff --git a/system/ui/text.py b/system/ui/text.py index 97d0ebc..4ea886b 100755 --- a/system/ui/text.py +++ b/system/ui/text.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import re import pyray as rl from openpilot.system.hardware import HARDWARE from openpilot.system.ui.lib.button import gui_button, ButtonStyle @@ -7,8 +8,8 @@ from openpilot.system.ui.lib.application import gui_app MARGIN = 50 SPACING = 50 -FONT_SIZE = 60 -LINE_HEIGHT = 64 +FONT_SIZE = 72 +LINE_HEIGHT = 80 BUTTON_SIZE = rl.Vector2(310, 160) DEMO_TEXT = """This is a sample text that will be wrapped and scrolled if necessary. @@ -16,18 +17,24 @@ DEMO_TEXT = """This is a sample text that will be wrapped and scrolled if necess def wrap_text(text, font_size, max_width): lines = [] - current_line = "" font = gui_app.font() - for word in text.split(): - test_line = current_line + word + " " - if rl.measure_text_ex(font, test_line, font_size, 0).x <= max_width: - current_line = test_line - else: + for paragraph in text.split("\n"): + if not paragraph.strip(): + lines.append("") + continue + indent = re.match(r"^\s*", paragraph).group() + current_line = indent + for word in paragraph.split(): + test_line = current_line + word + " " + if rl.measure_text_ex(font, test_line, font_size, 0).x <= max_width: + current_line = test_line + else: + lines.append(current_line) + current_line = word + " " + current_line = current_line.rstrip() + if current_line: lines.append(current_line) - current_line = word + " " - if current_line: - lines.append(current_line) return lines @@ -45,7 +52,7 @@ class TextWindow: position = rl.Vector2(self._textarea_rect.x + scroll.x, self._textarea_rect.y + scroll.y + i * LINE_HEIGHT) if position.y + LINE_HEIGHT < self._textarea_rect.y or position.y > self._textarea_rect.y + self._textarea_rect.height: continue - rl.draw_text_ex(gui_app.font(), line.strip(), position, FONT_SIZE, 0, rl.WHITE) + rl.draw_text_ex(gui_app.font(), line, position, FONT_SIZE, 0, rl.WHITE) rl.end_scissor_mode() button_bounds = rl.Rectangle(gui_app.width - MARGIN - BUTTON_SIZE.x, gui_app.height - MARGIN - BUTTON_SIZE.y, BUTTON_SIZE.x, BUTTON_SIZE.y) diff --git a/system/ui/updater.py b/system/ui/updater.py new file mode 100755 index 0000000..eb9d766 --- /dev/null +++ b/system/ui/updater.py @@ -0,0 +1,197 @@ +#!/usr/bin/env python3 +import sys +import subprocess +import threading +import pyray as rl +from enum import IntEnum + +from openpilot.system.hardware import HARDWARE +from openpilot.system.ui.lib.application import gui_app, FontWeight +from openpilot.system.ui.lib.button import gui_button, ButtonStyle +from openpilot.system.ui.lib.label import gui_text_box, gui_label + +# Constants +MARGIN = 50 +BUTTON_HEIGHT = 160 +BUTTON_WIDTH = 400 +PROGRESS_BAR_HEIGHT = 72 +TITLE_FONT_SIZE = 80 +BODY_FONT_SIZE = 65 +BACKGROUND_COLOR = rl.BLACK +PROGRESS_BG_COLOR = rl.Color(41, 41, 41, 255) +PROGRESS_COLOR = rl.Color(54, 77, 239, 255) + + +class Screen(IntEnum): + PROMPT = 0 + WIFI = 1 + PROGRESS = 2 + + +class Updater: + def __init__(self, updater_path, manifest_path): + self.updater = updater_path + self.manifest = manifest_path + self.current_screen = Screen.PROMPT + + self.progress_value = 0 + self.progress_text = "Loading..." + self.show_reboot_button = False + self.process = None + self.update_thread = None + + def install_update(self): + self.current_screen = Screen.PROGRESS + self.progress_value = 0 + self.progress_text = "Downloading..." + self.show_reboot_button = False + + # Start the update process in a separate thread + self.update_thread = threading.Thread(target=self._run_update_process) + self.update_thread.daemon = True + self.update_thread.start() + + def _run_update_process(self): + # TODO: just import it and run in a thread without a subprocess + cmd = [self.updater, "--swap", self.manifest] + self.process = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, + text=True, bufsize=1, universal_newlines=True) + + for line in self.process.stdout: + parts = line.strip().split(":") + if len(parts) == 2: + self.progress_text = parts[0] + try: + self.progress_value = int(float(parts[1])) + except ValueError: + pass + + exit_code = self.process.wait() + if exit_code == 0: + HARDWARE.reboot() + else: + self.progress_text = "Update failed" + self.show_reboot_button = True + + def render_prompt_screen(self): + # Title + title_rect = rl.Rectangle(MARGIN + 50, 250, gui_app.width - MARGIN * 2 - 100, TITLE_FONT_SIZE) + gui_label(title_rect, "Update Required", TITLE_FONT_SIZE, font_weight=FontWeight.BOLD) + + # Description + desc_text = "An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. \ + The download size is approximately 1GB." + desc_rect = rl.Rectangle(MARGIN + 50, 250 + TITLE_FONT_SIZE + 75, gui_app.width - MARGIN * 2 - 100, BODY_FONT_SIZE * 3) + gui_text_box(desc_rect, desc_text, BODY_FONT_SIZE) + + # Buttons at the bottom + button_y = gui_app.height - MARGIN - BUTTON_HEIGHT + button_width = (gui_app.width - MARGIN * 3) // 2 + + # WiFi button + wifi_button_rect = rl.Rectangle(MARGIN, button_y, button_width, BUTTON_HEIGHT) + if gui_button(wifi_button_rect, "Connect to Wi-Fi"): + self.current_screen = Screen.WIFI + return # Return to avoid processing other buttons after screen change + + # Install button + install_button_rect = rl.Rectangle(MARGIN * 2 + button_width, button_y, button_width, BUTTON_HEIGHT) + if gui_button(install_button_rect, "Install", button_style=ButtonStyle.PRIMARY): + self.install_update() + return # Return to avoid further processing after action + + def render_wifi_screen(self): + # Title and back button + title_rect = rl.Rectangle(MARGIN + 50, MARGIN, gui_app.width - MARGIN * 2 - 100, 60) + gui_label(title_rect, "Wi-Fi Networks", 60, font_weight=FontWeight.BOLD) + + back_button_rect = rl.Rectangle(MARGIN, gui_app.height - MARGIN - BUTTON_HEIGHT, BUTTON_WIDTH, BUTTON_HEIGHT) + if gui_button(back_button_rect, "Back"): + self.current_screen = Screen.PROMPT + return # Return to avoid processing other interactions after screen change + + # Draw placeholder for WiFi implementation + placeholder_rect = rl.Rectangle( + MARGIN, + title_rect.y + title_rect.height + MARGIN, + gui_app.width - MARGIN * 2, + gui_app.height - title_rect.height - MARGIN * 3 - BUTTON_HEIGHT + ) + + # Draw rounded rectangle background + rl.draw_rectangle_rounded( + placeholder_rect, + 0.1, + 10, + rl.Color(41, 41, 41, 255) + ) + + # Draw placeholder text + placeholder_text = "WiFi Implementation Placeholder" + text_size = rl.measure_text_ex(gui_app.font(), placeholder_text, 80, 1) + text_pos = rl.Vector2( + placeholder_rect.x + (placeholder_rect.width - text_size.x) / 2, + placeholder_rect.y + (placeholder_rect.height - text_size.y) / 2 + ) + rl.draw_text_ex(gui_app.font(), placeholder_text, text_pos, 80, 1, rl.WHITE) + + # Draw instructions + instructions_text = "Real WiFi functionality would be implemented here" + instructions_size = rl.measure_text_ex(gui_app.font(), instructions_text, 40, 1) + instructions_pos = rl.Vector2( + placeholder_rect.x + (placeholder_rect.width - instructions_size.x) / 2, + text_pos.y + text_size.y + 20 + ) + rl.draw_text_ex(gui_app.font(), instructions_text, instructions_pos, 40, 1, rl.GRAY) + + def render_progress_screen(self): + title_rect = rl.Rectangle(MARGIN + 100, 330, gui_app.width - MARGIN * 2 - 200, 100) + gui_label(title_rect, self.progress_text, 90, font_weight=FontWeight.SEMI_BOLD) + + # Progress bar + bar_rect = rl.Rectangle(MARGIN + 100, 330 + 100 + 100, gui_app.width - MARGIN * 2 - 200, PROGRESS_BAR_HEIGHT) + rl.draw_rectangle_rounded(bar_rect, 0.5, 10, PROGRESS_BG_COLOR) + + # Calculate the width of the progress chunk + progress_width = (bar_rect.width * self.progress_value) / 100 + if progress_width > 0: + progress_rect = rl.Rectangle(bar_rect.x, bar_rect.y, progress_width, bar_rect.height) + rl.draw_rectangle_rounded(progress_rect, 0.5, 10, PROGRESS_COLOR) + + # Show reboot button if needed + if self.show_reboot_button: + reboot_rect = rl.Rectangle(MARGIN + 100, gui_app.height - MARGIN - BUTTON_HEIGHT, BUTTON_WIDTH, BUTTON_HEIGHT) + if gui_button(reboot_rect, "Reboot"): + # Return True to signal main loop to exit before rebooting + HARDWARE.reboot() + return + + def render(self): + if self.current_screen == Screen.PROMPT: + self.render_prompt_screen() + elif self.current_screen == Screen.WIFI: + self.render_wifi_screen() + elif self.current_screen == Screen.PROGRESS: + self.render_progress_screen() + + +def main(): + if len(sys.argv) < 3: + print("Usage: updater.py ") + sys.exit(1) + + updater_path = sys.argv[1] + manifest_path = sys.argv[2] + + try: + gui_app.init_window("System Update") + updater = Updater(updater_path, manifest_path) + for _ in gui_app.render(): + updater.render() + finally: + # Make sure we clean up even if there's an error + gui_app.close() + + +if __name__ == "__main__": + main()