diff --git a/common/params.cc b/common/params.cc index 38004ea..5dec4b4 100644 --- a/common/params.cc +++ b/common/params.cc @@ -217,8 +217,6 @@ std::unordered_map keys = { {"GMapKey", PERSISTENT}, {"SearchInput", PERSISTENT}, - - {"CarSelected3", PERSISTENT}, {"SupportedCars", PERSISTENT}, {"SupportedCars_gm", PERSISTENT}, @@ -236,6 +234,8 @@ std::unordered_map keys = { {"ShowPathModeLane", PERSISTENT}, {"ShowPathColorLane", PERSISTENT}, {"ShowPlotMode", PERSISTENT}, + {"RecordRoadCam", PERSISTENT }, + {"AutoCruiseControl", PERSISTENT}, {"CruiseEcoControl", PERSISTENT}, {"AutoGasTokSpeed", PERSISTENT}, @@ -309,9 +309,11 @@ std::unordered_map keys = { {"LaneChangeNeedTorque", PERSISTENT}, {"MaxAngleFrames", PERSISTENT}, {"SoftHoldMode", PERSISTENT}, - {"CarrotLatControl", PERSISTENT }, + {"CarrotLatControl", PERSISTENT}, + {"CarrotLatFilter", PERSISTENT}, {"LatMpcPathCost", PERSISTENT }, {"LatMpcMotionCost", PERSISTENT }, + {"LatMpcMotionCost2", PERSISTENT }, {"LatMpcAccelCost", PERSISTENT}, {"LatMpcJerkCost", PERSISTENT}, {"LatMpcSteeringRateCost", PERSISTENT}, diff --git a/selfdrive/carrot_settings.json b/selfdrive/carrot_settings.json index c6633ef..0820a4c 100644 --- a/selfdrive/carrot_settings.json +++ b/selfdrive/carrot_settings.json @@ -66,6 +66,19 @@ "default": 1, "unit": 1 }, + { + "group": "조향튜닝", + "name": "CarrotLatFilter", + "title": "*당근조향제어필터(5)", + "descr": "필터링개수", + "egroup": "LAT", + "etitle": "*Carrot lateral filter(5)", + "edescr": "", + "min": 2, + "max": 30, + "default": 5, + "unit": 1 + }, { "group": "조향튜닝", "name": "LatMpcPathCost", @@ -92,6 +105,19 @@ "default": 11, "unit": 1 }, + { + "group": "조향튜닝", + "name": "LatMpcMotionCost2", + "title": "*LATMPC.MotionCost2(11)", + "descr": "헤딩가중치", + "egroup": "LAT", + "etitle": "*LATMPC.MotionCost2(11)", + "edescr": "", + "min": 0, + "max": 500, + "default": 11, + "unit": 1 + }, { "group": "조향튜닝", "name": "LatMpcAccelCost", @@ -625,6 +651,19 @@ "default": 0, "unit": 1 }, + { + "group": "시작", + "name": "RecordRoadCam", + "title": "RoadCam녹화", + "descr": "1:RoadCam, 2:WideRoadCam", + "egroup": "START", + "etitle": "Record RoadCam", + "edescr": "1:RoadCam, 2:WideRoadCam", + "min": 0, + "max": 2, + "default": 0, + "unit": 1 + }, { "group": "시작", "name": "MapboxStyle", diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 468815e..27de8de 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -74,8 +74,12 @@ class LateralPlanner: self.curve_speed = 0 self.prev_path_xyz = None - self.path_history = deque(maxlen=5) self.carrot_lat_control = 0 + self.carrot_lat_filter = 5 + self.path_history = deque(maxlen=self.carrot_lat_filter) + + self.lateralMotionCost = LATERAL_MOTION_COST + self.lateralMotionCost2 = LATERAL_MOTION_COST def reset_mpc(self, x0=None): if x0 is None: @@ -84,18 +88,23 @@ class LateralPlanner: self.lat_mpc.reset(x0=self.x0) def update(self, sm): - global PATH_COST, LATERAL_ACCEL_COST, LATERAL_JERK_COST, STEERING_RATE_COST, LATERAL_MOTION_COST + global PATH_COST, LATERAL_ACCEL_COST, LATERAL_JERK_COST, STEERING_RATE_COST self.readParams -= 1 if self.readParams <= 0: self.readParams = 100 self.useLaneLineSpeedApply = self.params.get_int("UseLaneLineSpeedApply") self.pathOffset = 0.0 #float(self.params.get_int("PathOffset")) * 0.01 PATH_COST = self.params.get_float("LatMpcPathCost") * 0.01 - LATERAL_MOTION_COST = self.params.get_float("LatMpcMotionCost") * 0.01 + self.lateralMotionCost = self.params.get_float("LatMpcMotionCost") * 0.01 + self.lateralMotionCost2 = self.params.get_float("LatMpcMotionCost2") * 0.01 LATERAL_ACCEL_COST = self.params.get_float("LatMpcAccelCost") * 0.01 LATERAL_JERK_COST = self.params.get_float("LatMpcJerkCost") * 0.01 STEERING_RATE_COST = self.params.get_float("LatMpcSteeringRateCost") self.carrot_lat_control = self.params.get_int("CarrotLatControl") + carrot_lat_filter = self.params.get_int("CarrotLatFilter") + if carrot_lat_filter != self.carrot_lat_filter: + self.carrot_lat_filter = carrot_lat_filter + self.path_history = deque(maxlen=self.carrot_lat_filter) # clip speed , lateral planning is not possible at 0 speed measured_curvature = sm['controlsState'].curvature @@ -140,6 +149,14 @@ class LateralPlanner: else: self.LP.lane_change_multiplier = 1.0 + lateral_motion_cost = self.lateralMotionCost + atc_type = sm['carrotMan'].atcType + if atc_activate: + if atc_type == "turn left" and md.meta.desireState[1] > 0.1: + lateral_motion_cost = self.lateralMotionCost2 + elif atc_type == "turn right" and md.meta.desireState[2] > 0.1: + lateral_motion_cost = self.lateralMotionCost2 + # lanelines calculation? self.LP.lanefull_mode = self.useLaneLineMode self.LP.lane_width_left = md.meta.laneWidthLeft @@ -161,15 +178,14 @@ class LateralPlanner: self.path_xyz = self.alpha * self.path_xyz + (1 - self.alpha) * self.prev_path_xyz self.prev_path_xyz = self.path_xyz """ - if self.carrot_lat_control in [1,2]: - if self.plan_a[0] < -1.0: + if False: #self.plan_a[0] < -1.0: self.path_history.clear() self.path_history.append(self.path_xyz) self.path_xyz = np.mean(np.array(self.path_history), axis=0) - - self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST, + + self.lat_mpc.set_weights(PATH_COST, lateral_motion_cost, LATERAL_ACCEL_COST, LATERAL_JERK_COST, STEERING_RATE_COST) diff --git a/selfdrive/navd/map_renderer.cc b/selfdrive/navd/map_renderer.cc index 64ecfd7..541ec11 100644 --- a/selfdrive/navd/map_renderer.cc +++ b/selfdrive/navd/map_renderer.cc @@ -128,34 +128,26 @@ MapRenderer::MapRenderer(const QMapLibre::Settings &settings, bool online) : m_s void MapRenderer::msgUpdate() { sm->update(1000); - float lat = 0.0; - float lon = 0.0; - float bearing = 0.0; - bool gps_updated = false; auto carrotMan = (*sm)["carrotMan"].getCarrotMan(); bool active_carrot_man = carrotMan.getActiveCarrot() > 1; - if (!active_carrot_man && sm->updated("liveLocationKalman")) { + if (sm->updated("liveLocationKalman")) { auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman(); auto pos = location.getPositionGeodetic(); auto orientation = location.getCalibratedOrientationNED(); if ((sm->rcv_frame("liveLocationKalman") % LLK_DECIMATION) == 0) { - bearing = RAD2DEG(orientation.getValue()[2]); - lat = pos.getValue()[0]; - lon = pos.getValue()[1]; - gps_updated = true; - } - } - if (active_carrot_man && sm->updated("carrotMan")) { - bearing = carrotMan.getXPosAngle(); - lat = carrotMan.getXPosLat(); - lon = carrotMan.getXPosLon(); - gps_updated = true; - } + float bearing = RAD2DEG(orientation.getValue()[2]); + float lat = pos.getValue()[0]; + float lon = pos.getValue()[1]; + + if (active_carrot_man) { + bearing = carrotMan.getXPosAngle(); + lat = carrotMan.getXPosLat(); + lon = carrotMan.getXPosLon(); + } - if(gps_updated) { - updatePosition(get_point_along_line(lat, lon, bearing, MAP_OFFSET), bearing); + updatePosition(get_point_along_line(lat, lon, bearing, MAP_OFFSET), bearing); // TODO: use the static rendering mode instead // retry render a few times for (int i = 0; i < 5 && !rendered(); i++) { @@ -171,7 +163,9 @@ void MapRenderer::msgUpdate() { // fallback to sending a blank frame if (!rendered()) { publish(0, false); + printf("blank frame\n"); } + } } if (sm->updated("navRoute")) { @@ -234,6 +228,9 @@ void MapRenderer::publish(const double render_time, const bool loaded) { auto carrotMan = (*sm)["carrotMan"].getCarrotMan(); auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman(); bool valid = loaded && ((carrotMan.getActiveCarrot() > 1) || ((location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && location.getPositionGeodetic().getValid())); + if (!valid) { + printf("loaded %d, active_carrot %d, status %d, valid %d\n", loaded, carrotMan.getActiveCarrot(), (int)location.getStatus(), location.getPositionGeodetic().getValid()); + } ever_loaded = ever_loaded || loaded; uint64_t ts = nanos_since_boot(); VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_MAP); diff --git a/selfdrive/navd/map_renderer.h b/selfdrive/navd/map_renderer.h index 6053819..ef29bb9 100644 --- a/selfdrive/navd/map_renderer.h +++ b/selfdrive/navd/map_renderer.h @@ -46,7 +46,7 @@ private: uint32_t frame_id = 0; uint64_t last_llk_rendered = 0; bool rendered() { - return last_llk_rendered == (*sm)["carrotMan"].getLogMonoTime(); + return last_llk_rendered == (*sm)["liveLocationKalman"].getLogMonoTime(); } QTimer* timer; diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 9baf2da..c2a7634 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -776,6 +776,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) { startToggles->addItem(new ParamControl("DisableDM", "Disable DM", "", "../assets/img_driver_face_static_x.png", this)); //startToggles->addItem(new CValueControl("CarrotCountDownSpeed", "NaviCountDown Speed(10)", "", "../assets/offroad/icon_shell.png", 0, 200, 5)); startToggles->addItem(new CValueControl("MapboxStyle", "Mapbox Style(0)", "", "../assets/offroad/icon_shell.png", 0, 2, 1)); + startToggles->addItem(new CValueControl("RecordRoadCam", "Record Road camera(0)", "1:RoadCam, 2:RoadCam+WideRoadCam", "../assets/offroad/icon_shell.png", 0, 2, 1)); startToggles->addItem(new ParamControl("HotspotOnBoot", "Hotspot enabled on boot", "", "../assets/offroad/icon_shell.png", this)); //startToggles->addItem(new ParamControl("NoLogging", "Disable Logger", "", "../assets/offroad/icon_shell.png", this)); //startToggles->addItem(new ParamControl("LaneChangeNeedTorque", "LaneChange: Need Torque", "", "../assets/offroad/icon_shell.png", this)); diff --git a/system/loggerd/loggerd.h b/system/loggerd/loggerd.h index 009338a..c70ad0e 100644 --- a/system/loggerd/loggerd.h +++ b/system/loggerd/loggerd.h @@ -57,21 +57,21 @@ public: const EncoderInfo main_road_encoder_info = { .publish_name = "roadEncodeData", .filename = "fcamera.hevc", - .record = false, + .record = Params().getInt("RecordRoadCam") > 0, INIT_ENCODE_FUNCTIONS(RoadEncode), }; const EncoderInfo main_wide_road_encoder_info = { .publish_name = "wideRoadEncodeData", .filename = "ecamera.hevc", - .record = false, + .record = Params().getInt("RecordRoadCam") > 1, INIT_ENCODE_FUNCTIONS(WideRoadEncode), }; const EncoderInfo main_driver_encoder_info = { .publish_name = "driverEncodeData", .filename = "dcamera.hevc", - .record = false, //Params().getBool("RecordFront"), + .record = Params().getBool("RecordFront"), INIT_ENCODE_FUNCTIONS(DriverEncode), }; diff --git a/system/manager/manager.py b/system/manager/manager.py index 41abdeb..bdeeadb 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -124,6 +124,7 @@ def get_default_params(): ("LaneChangeNeedTorque", "0"), ("MaxAngleFrames", "89"), ("CarrotLatControl", "0"), + ("CarrotLatFilter", "5"), ("DampingFactor", "0"), ("LateralTorqueCustom", "0"), ("LateralTorqueAccelFactor", "2500"), @@ -131,6 +132,7 @@ def get_default_params(): ("LateralTorqueKd", "0"), ("LatMpcPathCost", "100"), ("LatMpcMotionCost", "11"), + ("LatMpcMotionCost2", "11"), ("LatMpcAccelCost", "0"), ("LatMpcJerkCost", "4"), ("LatMpcSteeringRateCost", "700"), @@ -142,6 +144,7 @@ def get_default_params(): ("ModelActuatorDelay", "20"), ("MaxTimeOffroadMin", "60"), ("DisableDM", "0"), + ("RecordRoadCam", "0"), ("CruiseOnDist", "400"), ("HotspotOnBoot", "0"), ("CustomSR", "0"),