zhudongjer/carrot
1
1
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

VegoStopping setting

Browse Source 
Nav speed control mode...
fix. some
This commit is contained in:
ajouatom 2025-02-24 10:08:08 +09:00
parent 136ef70b3b
commit f2dcc4205d
10 changed files with 93 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
common/params.cc
View File

@ -251,6 +251,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"GpsDelayTimeAdjust", PERSISTENT}, {"GpsDelayTimeAdjust", PERSISTENT},
{"AutoTurnMapChange", PERSISTENT }, {"AutoTurnMapChange", PERSISTENT },
{"AutoNaviSpeedCtrlEnd", PERSISTENT}, {"AutoNaviSpeedCtrlEnd", PERSISTENT},
{"AutoNaviSpeedCtrlMode", PERSISTENT},
{"AutoNaviSpeedBumpTime", PERSISTENT}, {"AutoNaviSpeedBumpTime", PERSISTENT},
{"AutoNaviSpeedBumpSpeed", PERSISTENT}, {"AutoNaviSpeedBumpSpeed", PERSISTENT},
{"AutoNaviSpeedDecelRate", PERSISTENT}, {"AutoNaviSpeedDecelRate", PERSISTENT},
@ -283,7 +284,8 @@ std::unordered_map<std::string, uint32_t> keys = {
{"LongTuningKpV", PERSISTENT}, {"LongTuningKpV", PERSISTENT},
{"LongTuningKiV", PERSISTENT}, {"LongTuningKiV", PERSISTENT},
{"LongTuningKf", PERSISTENT}, {"LongTuningKf", PERSISTENT},
{"LongActuatorDelay", PERSISTENT}, {"LongActuatorDelay", PERSISTENT },
{"VegoStopping", PERSISTENT },
{"RadarReactionFactor", PERSISTENT}, {"RadarReactionFactor", PERSISTENT},
{"EnableRadarTracks", PERSISTENT}, {"EnableRadarTracks", PERSISTENT},
{"EnableRadarTracksResult", PERSISTENT | CLEAR_ON_MANAGER_START}, {"EnableRadarTracksResult", PERSISTENT | CLEAR_ON_MANAGER_START},

50
selfdrive/carrot/carrot_man.py
View File

@ -182,7 +182,7 @@ class CarrotMan:
print("************************************************CarrotMan init************************************************") print("************************************************CarrotMan init************************************************")
self.params = Params() self.params = Params()
self.params_memory = Params("/dev/shm/params") self.params_memory = Params("/dev/shm/params")
self.sm = messaging.SubMaster(['deviceState', 'carState', 'controlsState', 'longitudinalPlan', 'modelV2', 'selfdriveState', 'carControl', 'navRouteNavd']) self.sm = messaging.SubMaster(['deviceState', 'carState', 'controlsState', 'longitudinalPlan', 'modelV2', 'selfdriveState', 'carControl', 'navRouteNavd', 'liveLocationKalman'])
self.pm = messaging.PubMaster(['carrotMan', "navRoute", "navInstructionCarrot"]) self.pm = messaging.PubMaster(['carrotMan', "navRoute", "navInstructionCarrot"])
self.carrot_serv = CarrotServ() self.carrot_serv = CarrotServ()
@ -895,7 +895,6 @@ class CarrotServ:
self.nPosAngle = 0.0 self.nPosAngle = 0.0
self.diff_angle_count = 0 self.diff_angle_count = 0
self.last_update_gps_time = 0
self.last_calculate_gps_time = 0 self.last_calculate_gps_time = 0
self.bearing_offset = 0.0 self.bearing_offset = 0.0
self.bearing_measured = 0.0 self.bearing_measured = 0.0
@ -948,6 +947,7 @@ class CarrotServ:
self.autoNaviSpeedBumpSpeed = float(self.params.get_int("AutoNaviSpeedBumpSpeed")) self.autoNaviSpeedBumpSpeed = float(self.params.get_int("AutoNaviSpeedBumpSpeed"))
self.autoNaviSpeedBumpTime = float(self.params.get_int("AutoNaviSpeedBumpTime")) self.autoNaviSpeedBumpTime = float(self.params.get_int("AutoNaviSpeedBumpTime"))
self.autoNaviSpeedCtrlEnd = float(self.params.get_int("AutoNaviSpeedCtrlEnd")) self.autoNaviSpeedCtrlEnd = float(self.params.get_int("AutoNaviSpeedCtrlEnd"))
self.autoNaviSpeedCtrlMode = self.params.get_int("AutoNaviSpeedCtrlMode")
self.autoNaviSpeedSafetyFactor = float(self.params.get_int("AutoNaviSpeedSafetyFactor")) * 0.01 self.autoNaviSpeedSafetyFactor = float(self.params.get_int("AutoNaviSpeedSafetyFactor")) * 0.01
self.autoNaviSpeedDecelRate = float(self.params.get_int("AutoNaviSpeedDecelRate")) * 0.01 self.autoNaviSpeedDecelRate = float(self.params.get_int("AutoNaviSpeedDecelRate")) * 0.01
self.autoNaviCountDownMode = self.params.get_int("AutoNaviCountDownMode") self.autoNaviCountDownMode = self.params.get_int("AutoNaviCountDownMode")
@ -1205,16 +1205,17 @@ class CarrotServ:
# 1: startOSEPS: 구간단속시작 # 1: startOSEPS: 구간단속시작
# 2: inOSEPS: 구간단속중 # 2: inOSEPS: 구간단속중
# 3: endOSEPS: 구간단속종료 # 3: endOSEPS: 구간단속종료
if self.nSdiType in [0,1,2,3,4,7,8, 75, 76] and self.nSdiSpeedLimit > 0: # 0:감속안함,1:과속카메라,2:+사고방지턱,3:+이동식카메라
if self.nSdiType in [0,1,2,3,4,7,8, 75, 76] and self.nSdiSpeedLimit > 0 and self.autoNaviSpeedCtrlMode > 0:
self.xSpdLimit = self.nSdiSpeedLimit * self.autoNaviSpeedSafetyFactor self.xSpdLimit = self.nSdiSpeedLimit * self.autoNaviSpeedSafetyFactor
self.xSpdDist = self.nSdiDist self.xSpdDist = self.nSdiDist
self.xSpdType = self.nSdiType self.xSpdType = self.nSdiType
if self.nSdiBlockType in [2,3]: if self.nSdiBlockType in [2,3]:
self.xSpdDist = self.nSdiBlockDist self.xSpdDist = self.nSdiBlockDist
self.xSpdType = 4 self.xSpdType = 4
elif self.nSdiType == 7: #이동식카메라 elif self.nSdiType == 7 and self.autoNaviSpeedCtrlMode < 3: #이동식카메라
self.xSpdLimit = self.xSpdDist = 0 self.xSpdLimit = self.xSpdDist = 0
elif (self.nSdiPlusType == 22 or self.nSdiType == 22) and self.roadcate > 1: # speed bump, roadcate:0,1: highway elif (self.nSdiPlusType == 22 or self.nSdiType == 22) and self.roadcate > 1 and self.autoNaviSpeedCtrlMode >= 2: # speed bump, roadcate:0,1: highway
self.xSpdLimit = self.autoNaviSpeedBumpSpeed self.xSpdLimit = self.autoNaviSpeedBumpSpeed
self.xSpdDist = self.nSdiPlusDist if self.nSdiPlusType == 22 else self.nSdiDist self.xSpdDist = self.nSdiPlusDist if self.nSdiPlusType == 22 else self.nSdiDist
self.xSpdType = 22 self.xSpdType = 22
@ -1224,22 +1225,30 @@ class CarrotServ:
self.xSpdDist = 0 self.xSpdDist = 0
def _update_gps(self, v_ego, sm): def _update_gps(self, v_ego, sm):
if not sm.updated['carState'] or not sm.updated['carControl']: llk = 'liveLocationKalman'
location = sm[llk]
#print(f"location = {sm.valid[llk]}, {sm.updated[llk]}, {sm.recv_frame[llk]}, {sm.recv_time[llk]}")
if not sm.updated['carState'] or not sm.updated['carControl'] or not sm.updated[llk]:
return self.nPosAngle return self.nPosAngle
CS = sm['carState'] CS = sm['carState']
CC = sm['carControl'] CC = sm['carControl']
if len(CC.orientationNED) == 3: self.gps_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
bearing = math.degrees(CC.orientationNED[2])
now = time.monotonic()
if sm.valid[llk]:
bearing = math.degrees(location.calibratedOrientationNED.value[2])
else: else:
bearing = 0.0 bearing = self.nPosAngle
return self.nPosAngle
if not self.gps_valid:
if self.params_memory.get("LastGPSPosition"):
self.gps_valid = True
#print(f"gps_valid = {self.gps_valid}, bearing = {bearing:.1f}, pos = {location.positionGeodetic.value[0]:.6f}, {location.positionGeodetic.value[1]:.6f}")
if self.gps_valid: # liveLocationKalman일때는 정확하나, livePose일때는 불안정함. if self.gps_valid: # liveLocationKalman일때는 정확하나, livePose일때는 불안정함.
self.bearing_offset = 0.0 self.bearing_offset = 0.0
if self.active_carrot <= 1:
self.vpPosPointLatNavi = location.positionGeodetic.value[0]
self.vpPosPointLonNavi = location.positionGeodetic.value[1]
self.last_calculate_gps_time = sm.recv_time[llk]
self.gpsDelayTimeAdjust = 0.0
else: else:
if abs(self.bearing_measured - bearing) < 0.1: if abs(self.bearing_measured - bearing) < 0.1:
self.diff_angle_count += 1 self.diff_angle_count += 1
@ -1255,10 +1264,14 @@ class CarrotServ:
bearing_calculated = (bearing + self.bearing_offset) % 360 bearing_calculated = (bearing + self.bearing_offset) % 360
now = time.monotonic()
dt = now - self.last_calculate_gps_time dt = now - self.last_calculate_gps_time
#self.last_calculate_gps_time = now #print(f"dt = {dt:.1f}, {self.vpPosPointLatNavi}, {self.vpPosPointLonNavi}")
self.vpPosPointLat, self.vpPosPointLon = self.estimate_position(float(self.vpPosPointLatNavi), float(self.vpPosPointLonNavi), v_ego, bearing_calculated, dt + self.gpsDelayTimeAdjust) if dt > 5.0:
self.vpPosPointLat, self.vpPosPointLon = 0.0, 0.0
elif dt == 0:
self.vpPosPointLat, self.vpPosPointLon = self.vpPosPointLatNavi, self.vpPosPointLonNavi
else:
self.vpPosPointLat, self.vpPosPointLon = self.estimate_position(float(self.vpPosPointLatNavi), float(self.vpPosPointLonNavi), v_ego, bearing_calculated, dt + self.gpsDelayTimeAdjust)
#self.debugText = " {} {:.1f},{:.1f}={:.1f}+{:.1f}".format(self.active_sdi_count, self.nPosAngle, bearing_calculated, bearing, self.bearing_offset) #self.debugText = " {} {:.1f},{:.1f}={:.1f}+{:.1f}".format(self.active_sdi_count, self.nPosAngle, bearing_calculated, bearing, self.bearing_offset)
#print("nPosAngle = {:.1f},{:.1f} = {:.1f}+{:.1f}".format(self.nPosAngle, bearing_calculated, bearing, self.bearing_offset)) #print("nPosAngle = {:.1f},{:.1f} = {:.1f}+{:.1f}".format(self.nPosAngle, bearing_calculated, bearing, self.bearing_offset))
@ -1397,7 +1410,7 @@ class CarrotServ:
sdi_speed = 250 sdi_speed = 250
hda_active = False hda_active = False
### 과속카메라, 사고방지턱 ### 과속카메라, 사고방지턱
if self.xSpdDist > 0 and self.active_carrot > 0: if self.xSpdDist > 0 and self.active_carrot > 0:
safe_sec = self.autoNaviSpeedBumpTime if self.xSpdType == 22 else self.autoNaviSpeedCtrlEnd safe_sec = self.autoNaviSpeedBumpTime if self.xSpdType == 22 else self.autoNaviSpeedCtrlEnd
decel = self.autoNaviSpeedDecelRate decel = self.autoNaviSpeedDecelRate
@ -1510,7 +1523,6 @@ class CarrotServ:
self._update_cmd() self._update_cmd()
msg = messaging.new_message('carrotMan') msg = messaging.new_message('carrotMan')
msg.valid = True msg.valid = True
msg.carrotMan.activeCarrot = self.active_carrot msg.carrotMan.activeCarrot = self.active_carrot

26
selfdrive/carrot_settings.json
View File

@ -495,6 +495,19 @@
"default": 20, "default": 20,
"unit": 5 "unit": 5
}, },
{
"group": "주행튜닝",
"name": "VegoStopping",
"title": "정지출발민감도(5)x0.01",
"descr": "값을 올리면 늦게 출발함, 너무낮추면 추돌위험이 있을수 있음.",
"egroup": "LONG",
"etitle": "VegoStopping(5)x0.01",
"edescr": "",
"min": 1,
"max": 10,
"default": 20,
"unit": 5
},
{ {
"group": "주행튜닝", "group": "주행튜닝",
"name": "RadarReactionFactor", "name": "RadarReactionFactor",
@ -1367,6 +1380,19 @@
"default": 6, "default": 6,
"unit": 1 "unit": 1
}, },
{
"group": "감속제어",
"name": "AutoNaviSpeedCtrlMode",
"title": "네비게이션감속모드(2)",
"descr": "0:감속안함,1:과속카메라,2:+사고방지턱,3:+이동식카메라",
"egroup": "SPEED",
"etitle": "NaviSpeedControlMode(2)",
"edescr": "",
"min": 0,
"max": 3,
"default": 2,
"unit": 1
},
{ {
"group": "감속제어", "group": "감속제어",
"name": "AutoNaviSpeedBumpTime", "name": "AutoNaviSpeedBumpTime",

3
selfdrive/controls/lib/longitudinal_planner.py
View File

@ -217,10 +217,11 @@ class LongitudinalPlanner:
longitudinalPlan.fcw = self.fcw longitudinalPlan.fcw = self.fcw
longitudinalActuatorDelay = Params().get_float("LongActuatorDelay")*0.01 longitudinalActuatorDelay = Params().get_float("LongActuatorDelay")*0.01
vegoStopping = Params().get_float("VegoStopping") * 0.01
action_t = longitudinalActuatorDelay + DT_MDL action_t = longitudinalActuatorDelay + DT_MDL
a_target, should_stop, v_target, j_target = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels, longitudinalPlan.jerks, a_target, should_stop, v_target, j_target = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels, longitudinalPlan.jerks,
action_t=action_t, vEgoStopping=self.CP.vEgoStopping) action_t=action_t, vEgoStopping=vegoStopping)
longitudinalPlan.aTarget = float(a_target) longitudinalPlan.aTarget = float(a_target)
longitudinalPlan.shouldStop = bool(should_stop) longitudinalPlan.shouldStop = bool(should_stop)
longitudinalPlan.allowBrake = True longitudinalPlan.allowBrake = True

37
selfdrive/navd/map_renderer.cc
View File

@ -24,7 +24,7 @@ const int PIXELS_PER_TILE = 256;
const int MAP_OFFSET = 128; const int MAP_OFFSET = 128;
const bool TEST_MODE = getenv("MAP_RENDER_TEST_MODE"); const bool TEST_MODE = getenv("MAP_RENDER_TEST_MODE");
const int LLK_DECIMATION = TEST_MODE ? 1 : 10; //const int LLK_DECIMATION = TEST_MODE ? 1 : 10;
float get_zoom_level_for_scale(float lat, float meters_per_pixel) { float get_zoom_level_for_scale(float lat, float meters_per_pixel) {
float meters_per_tile = meters_per_pixel * PIXELS_PER_TILE; float meters_per_tile = meters_per_pixel * PIXELS_PER_TILE;
@ -110,7 +110,7 @@ MapRenderer::MapRenderer(const QMapLibre::Settings &settings, bool online) : m_s
vipc_server->start_listener(); vipc_server->start_listener();
pm.reset(new PubMaster({"navThumbnail", "mapRenderState"})); pm.reset(new PubMaster({"navThumbnail", "mapRenderState"}));
sm.reset(new SubMaster({"carrotMan", "liveLocationKalman", "navRoute"}, {"liveLocationKalman"})); sm.reset(new SubMaster({"carrotMan", "navRoute"}));
timer = new QTimer(this); timer = new QTimer(this);
timer->setSingleShot(true); timer->setSingleShot(true);
@ -123,23 +123,11 @@ void MapRenderer::msgUpdate() {
sm->update(1000); sm->update(1000);
auto carrotMan = (*sm)["carrotMan"].getCarrotMan(); auto carrotMan = (*sm)["carrotMan"].getCarrotMan();
bool active_carrot_man = carrotMan.getActiveCarrot() > 1;
if (sm->updated("liveLocationKalman")) { if (sm->updated("carrotMan")) {
auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman(); float bearing = carrotMan.getXPosAngle();
auto pos = location.getPositionGeodetic(); float lat = carrotMan.getXPosLat();
auto orientation = location.getCalibratedOrientationNED(); float lon = carrotMan.getXPosLon();
if ((sm->rcv_frame("liveLocationKalman") % LLK_DECIMATION) == 0) {
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();
}
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 // TODO: use the static rendering mode instead
@ -158,7 +146,6 @@ void MapRenderer::msgUpdate() {
publish(0, false); publish(0, false);
printf("blank frame\n"); printf("blank frame\n");
} }
}
} }
if (sm->updated("navRoute")) { if (sm->updated("navRoute")) {
@ -202,7 +189,7 @@ void MapRenderer::update() {
if ((vipc_server != nullptr) && loaded()) { if ((vipc_server != nullptr) && loaded()) {
publish((end_t - start_t) / 1000.0, true); publish((end_t - start_t) / 1000.0, true);
last_llk_rendered = (*sm)["liveLocationKalman"].getLogMonoTime(); last_llk_rendered = (*sm)["carrotMan"].getLogMonoTime();
} }
} }
@ -219,17 +206,13 @@ void MapRenderer::publish(const double render_time, const bool loaded) {
QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor); QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor);
auto carrotMan = (*sm)["carrotMan"].getCarrotMan(); auto carrotMan = (*sm)["carrotMan"].getCarrotMan();
auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman(); bool valid = loaded && (carrotMan.getXPosLat() > 0.0);
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; ever_loaded = ever_loaded || loaded;
uint64_t ts = nanos_since_boot(); uint64_t ts = nanos_since_boot();
VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_MAP); VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_MAP);
VisionIpcBufExtra extra = { VisionIpcBufExtra extra = {
.frame_id = frame_id, .frame_id = frame_id,
.timestamp_sof = (*sm)["liveLocationKalman"].getLogMonoTime(), .timestamp_sof = (*sm)["carrotMan"].getLogMonoTime(),
.timestamp_eof = ts, .timestamp_eof = ts,
.valid = valid, .valid = valid,
}; };
@ -278,7 +261,7 @@ void MapRenderer::publish(const double render_time, const bool loaded) {
auto evt = msg.initEvent(); auto evt = msg.initEvent();
auto state = evt.initMapRenderState(); auto state = evt.initMapRenderState();
evt.setValid(valid); evt.setValid(valid);
state.setLocationMonoTime((*sm)["liveLocationKalman"].getLogMonoTime()); state.setLocationMonoTime((*sm)["carrotMan"].getLogMonoTime());
state.setRenderTime(render_time); state.setRenderTime(render_time);
state.setFrameId(frame_id); state.setFrameId(frame_id);
pm->send("mapRenderState", msg); pm->send("mapRenderState", msg);

2
selfdrive/navd/map_renderer.h
View File

@ -46,7 +46,7 @@ private:
uint32_t frame_id = 0; uint32_t frame_id = 0;
uint64_t last_llk_rendered = 0; uint64_t last_llk_rendered = 0;
bool rendered() { bool rendered() {
return last_llk_rendered == (*sm)["liveLocationKalman"].getLogMonoTime(); return last_llk_rendered == (*sm)["carrotMan"].getLogMonoTime();
} }
QTimer* timer; QTimer* timer;

19
selfdrive/navd/navd.py
View File

@ -94,19 +94,14 @@ class RouteEngine:
cloudlog.exception("navd.failed_to_compute") cloudlog.exception("navd.failed_to_compute")
def update_location(self): def update_location(self):
location = self.sm['liveLocationKalman']
self.gps_ok = location.gpsOK
self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
carrot_man = self.sm['carrotMan'] carrot_man = self.sm['carrotMan']
if carrot_man.activeCarrot > 1: if carrot_man.xPosLat > 0.0:
self.last_bearing = carrot_man.xPosAngle; self.last_bearing = carrot_man.xPosAngle;
self.last_position = Coordinate(carrot_man.xPosLat, carrot_man.xPosLon) self.last_position = Coordinate(carrot_man.xPosLat, carrot_man.xPosLon)
self.gps_ok = True self.localizer_valid = self.gps_ok = True
elif self.localizer_valid: else:
self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2]) self.localizer_valid = self.gps_ok = False
self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
self.carrot_route_active = False self.carrot_route_active = False
def recompute_route(self): def recompute_route(self):
@ -119,10 +114,10 @@ class RouteEngine:
self.reset_recompute_limits() self.reset_recompute_limits()
return return
#print(f"new_destination: {new_destination} nav_destination: {self.nav_destination}") #print(f"new_destination: {new_destination} nav_destination: {self.nav_destination}")
should_recompute = self.should_recompute() should_recompute = self.should_recompute() and place_name != "External Navi"
if new_destination != self.nav_destination and place_name != "External Navi": if new_destination != self.nav_destination and place_name != "External Navi":
cloudlog.warning(f"Got new destination from NavDestination param {new_destination}") cloudlog.warning(f"Got new destination from NavDestination param {new_destination}")
print(f"Got new destination from NavDestination param {new_destination} {self.nav_destination}") print(f"Got new destination from NavDestination param {new_destination} {self.nav_destination} {place_name}")
should_recompute = True should_recompute = True
# Don't recompute when GPS drifts in tunnels # Don't recompute when GPS drifts in tunnels
@ -381,7 +376,7 @@ class RouteEngine:
def main(): def main():
pm = messaging.PubMaster(['navInstruction', 'navRouteNavd']) pm = messaging.PubMaster(['navInstruction', 'navRouteNavd'])
sm = messaging.SubMaster(['liveLocationKalman', 'managerState', 'carrotMan']) sm = messaging.SubMaster(['managerState', 'carrotMan'])
rk = Ratekeeper(1.0) rk = Ratekeeper(1.0)
route_engine = RouteEngine(sm, pm) route_engine = RouteEngine(sm, pm)

36
selfdrive/ui/qt/maps/map.cc
View File

@ -182,34 +182,14 @@ void MapWindow::updateState(const UIState &s) {
bool gps_updated = false; bool gps_updated = false;
auto carrotMan = sm["carrotMan"].getCarrotMan(); auto carrotMan = sm["carrotMan"].getCarrotMan();
bool active_carrot_man = carrotMan.getActiveCarrot() > 1; //bool active_carrot_man = carrotMan.getActiveCarrot() > 1;
if (!active_carrot_man && sm.updated("liveLocationKalman")) { //printf("CarrotMan: %f %f %f %f\n", carrotMan.getXPosLat(), carrotMan.getXPosLon(), carrotMan.getXPosAngle(), carrotMan.getXPosSpeed());
auto locationd_location = sm["liveLocationKalman"].getLiveLocationKalman(); if (carrotMan.getXPosLat() > 0.0) {
auto locationd_pos = locationd_location.getPositionGeodetic();
auto locationd_orientation = locationd_location.getCalibratedOrientationNED();
auto locationd_velocity = locationd_location.getVelocityCalibrated();
auto locationd_ecef = locationd_location.getPositionECEF();
locationd_valid = (locationd_pos.getValid() && locationd_orientation.getValid() && locationd_velocity.getValid() && locationd_ecef.getValid());
if (locationd_valid) {
// Check std norm
auto pos_ecef_std = locationd_ecef.getStd();
bool pos_accurate_enough = sqrt(pow(pos_ecef_std[0], 2) + pow(pos_ecef_std[1], 2) + pow(pos_ecef_std[2], 2)) < 100;
locationd_valid = pos_accurate_enough;
}
lat = locationd_pos.getValue()[0];
lon = locationd_pos.getValue()[1];
bearing = RAD2DEG(locationd_orientation.getValue()[2]);
velocity = locationd_velocity.getValue()[0];
gps_updated = true;
}
if (active_carrot_man && sm.updated("carrotMan")) {
lat = carrotMan.getXPosLat(); lat = carrotMan.getXPosLat();
lon = carrotMan.getXPosLon(); lon = carrotMan.getXPosLon();
bearing = carrotMan.getXPosAngle(); bearing = carrotMan.getXPosAngle();
velocity = carrotMan.getXPosSpeed()/3.6; velocity = carrotMan.getXPosSpeed() / 3.6;
gps_updated = true; gps_updated = true;
locationd_valid = true; locationd_valid = true;
} }
@ -249,7 +229,7 @@ void MapWindow::updateState(const UIState &s) {
initLayers(); initLayers();
if (!locationd_valid) { if (!locationd_valid) {
setError(tr("Waiting for APN")); setError(tr("Waiting for GPS(APN)"));
} else if (routing_problem) { } else if (routing_problem) {
setError(tr("Waiting for route")); setError(tr("Waiting for route"));
} else { } else {
@ -284,7 +264,7 @@ void MapWindow::updateState(const UIState &s) {
// - API exception/no internet // - API exception/no internet
// - route response is empty // - route response is empty
// - any time navd is waiting for recompute_countdown // - any time navd is waiting for recompute_countdown
routing_problem = !sm.valid("navInstruction") && coordinate_from_param("NavDestination").has_value(); routing_problem = !(sm.valid("navInstruction") || sm.valid("navInstructionCarrot")) && coordinate_from_param("NavDestination").has_value();
if (sm.valid("navInstruction") || sm.valid("navInstructionCarrot")) { if (sm.valid("navInstruction") || sm.valid("navInstructionCarrot")) {
//auto i = sm["navInstruction"].getNavInstruction(); //auto i = sm["navInstruction"].getNavInstruction();
@ -316,7 +296,9 @@ void MapWindow::updateState(const UIState &s) {
} }
if (loaded_once && (sm.rcv_frame("modelV2") != model_rcv_frame)) { if (loaded_once && (sm.rcv_frame("modelV2") != model_rcv_frame)) {
auto locationd_location = sm["liveLocationKalman"].getLiveLocationKalman(); auto locationd_location = sm["liveLocationKalman"].getLiveLocationKalman();
auto model_path = model_to_collection(locationd_location.getCalibratedOrientationECEF(), locationd_location.getPositionECEF(), sm["modelV2"].getModelV2().getPosition(), last_position->first, last_position->second); //auto carrot_man = sm["carrotMan"].getCarrotMan();
auto model_path = model_to_collection(locationd_location.getCalibratedOrientationECEF(), locationd_location.getPositionECEF(), sm["modelV2"].getModelV2().getPosition(), carrotMan.getXPosLat(), carrotMan.getXPosLon());
//auto model_path = model_to_collection(sm["modelV2"].getModelV2().getPosition(), carrotMan.getXPosLat(), carrotMan.getXPosLon(), carrotMan.getXPosAngle());
QMapLibre::Feature model_path_feature(QMapLibre::Feature::LineStringType, model_path, {}, {}); QMapLibre::Feature model_path_feature(QMapLibre::Feature::LineStringType, model_path, {}, {});
QVariantMap modelV2Path; QVariantMap modelV2Path;
modelV2Path["type"] = "geojson"; modelV2Path["type"] = "geojson";

2
selfdrive/ui/qt/offroad/settings.cc
View File

@ -663,6 +663,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
latLongToggles->addItem(new CValueControl("LongTuningKiV", "LONG: I Gain(200)", "", "../assets/offroad/icon_logic.png", 0, 2000, 5)); latLongToggles->addItem(new CValueControl("LongTuningKiV", "LONG: I Gain(200)", "", "../assets/offroad/icon_logic.png", 0, 2000, 5));
latLongToggles->addItem(new CValueControl("LongTuningKf", "LONG: FF Gain(100)", "", "../assets/offroad/icon_logic.png", 0, 200, 5)); latLongToggles->addItem(new CValueControl("LongTuningKf", "LONG: FF Gain(100)", "", "../assets/offroad/icon_logic.png", 0, 200, 5));
latLongToggles->addItem(new CValueControl("LongActuatorDelay", "LONG: ActuatorDelay(20)", "", "../assets/offroad/icon_logic.png", 0, 200, 5)); latLongToggles->addItem(new CValueControl("LongActuatorDelay", "LONG: ActuatorDelay(20)", "", "../assets/offroad/icon_logic.png", 0, 200, 5));
latLongToggles->addItem(new CValueControl("VegoStopping", "LONG: VEgoStopping(5)", "Stopping factor", "../assets/offroad/icon_logic.png", 1, 10, 1));
latLongToggles->addItem(new CValueControl("RadarReactionFactor", "LONG: Radar reaction factor(10)", "", "../assets/offroad/icon_logic.png", 0, 200, 10)); latLongToggles->addItem(new CValueControl("RadarReactionFactor", "LONG: Radar reaction factor(10)", "", "../assets/offroad/icon_logic.png", 0, 200, 10));
//latLongToggles->addItem(new CValueControl("StartAccelApply", "LONG: StartingAccel 2.0x(0)%", "정지->출발시 가속도의 가속율을 지정합니다 0: 사용안함.", "../assets/offroad/icon_road.png", 0, 100, 10)); //latLongToggles->addItem(new CValueControl("StartAccelApply", "LONG: StartingAccel 2.0x(0)%", "정지->출발시 가속도의 가속율을 지정합니다 0: 사용안함.", "../assets/offroad/icon_road.png", 0, 100, 10));
//latLongToggles->addItem(new CValueControl("StopAccelApply", "LONG: StoppingAccel -2.0x(0)%", "정지유지시 브레이크압을 조정합니다. 0: 사용안함. ", "../assets/offroad/icon_road.png", 0, 100, 10)); //latLongToggles->addItem(new CValueControl("StopAccelApply", "LONG: StoppingAccel -2.0x(0)%", "정지유지시 브레이크압을 조정합니다. 0: 사용안함. ", "../assets/offroad/icon_road.png", 0, 100, 10));
@ -801,6 +802,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
speedToggles->addItem(new CValueControl("AutoCurveSpeedFactor", "CURVE: Auto Control ratio(100%)", "", "../assets/offroad/icon_road.png", 50, 300, 1)); speedToggles->addItem(new CValueControl("AutoCurveSpeedFactor", "CURVE: Auto Control ratio(100%)", "", "../assets/offroad/icon_road.png", 50, 300, 1));
speedToggles->addItem(new CValueControl("AutoCurveSpeedAggressiveness", "CURVE: Aggressiveness (100%)", "", "../assets/offroad/icon_road.png", 50, 300, 1)); speedToggles->addItem(new CValueControl("AutoCurveSpeedAggressiveness", "CURVE: Aggressiveness (100%)", "", "../assets/offroad/icon_road.png", 50, 300, 1));
speedToggles->addItem(new CValueControl("AutoNaviSpeedCtrlEnd", "SpeedCameraDecelEnd(6s)", "감속완료시점을 설정합니다.값이 크면 카메라에서 멀리 감속 완료", "../assets/offroad/icon_road.png", 3, 20, 1)); speedToggles->addItem(new CValueControl("AutoNaviSpeedCtrlEnd", "SpeedCameraDecelEnd(6s)", "감속완료시점을 설정합니다.값이 크면 카메라에서 멀리 감속 완료", "../assets/offroad/icon_road.png", 3, 20, 1));
speedToggles->addItem(new CValueControl("AutoNaviSpeedCtrlMode", "0:감속안함,1:과속카메라,2:+사고방지턱,3:+이동식카메라", "../assets/offroad/icon_road.png", 0, 3, 1));
speedToggles->addItem(new CValueControl("AutoNaviSpeedDecelRate", "SpeedCameraDecelRatex0.01m/s^2(80)", "낮으면 멀리서부터 감속함", "../assets/offroad/icon_road.png", 10, 200, 10)); speedToggles->addItem(new CValueControl("AutoNaviSpeedDecelRate", "SpeedCameraDecelRatex0.01m/s^2(80)", "낮으면 멀리서부터 감속함", "../assets/offroad/icon_road.png", 10, 200, 10));
speedToggles->addItem(new CValueControl("AutoNaviSpeedSafetyFactor", "SpeedCameraSafetyFactor(105%)", "", "../assets/offroad/icon_road.png", 80, 120, 1)); speedToggles->addItem(new CValueControl("AutoNaviSpeedSafetyFactor", "SpeedCameraSafetyFactor(105%)", "", "../assets/offroad/icon_road.png", 80, 120, 1));
speedToggles->addItem(new CValueControl("AutoNaviSpeedBumpTime", "SpeedBumpTimeDistance(1s)", "", "../assets/offroad/icon_road.png", 1, 50, 1)); speedToggles->addItem(new CValueControl("AutoNaviSpeedBumpTime", "SpeedBumpTimeDistance(1s)", "", "../assets/offroad/icon_road.png", 1, 50, 1));

2
system/manager/manager.py
View File

@ -73,6 +73,7 @@ def get_default_params():
("AutoTurnMapChange", "0"), ("AutoTurnMapChange", "0"),
("AutoNaviSpeedCtrlEnd", "7"), ("AutoNaviSpeedCtrlEnd", "7"),
("AutoNaviSpeedCtrlMode", "2"),
("AutoNaviSpeedBumpTime", "1"), ("AutoNaviSpeedBumpTime", "1"),
("AutoNaviSpeedBumpSpeed", "35"), ("AutoNaviSpeedBumpSpeed", "35"),
("AutoNaviSpeedSafetyFactor", "105"), ("AutoNaviSpeedSafetyFactor", "105"),
@ -101,6 +102,7 @@ def get_default_params():
("LongTuningKiV", "0"), ("LongTuningKiV", "0"),
("LongTuningKf", "100"), ("LongTuningKf", "100"),
("LongActuatorDelay", "20"), ("LongActuatorDelay", "20"),
("VegoStopping", "5"),
("RadarReactionFactor", "10"), ("RadarReactionFactor", "10"),
("EnableRadarTracks", "0"), ("EnableRadarTracks", "0"),
("HyundaiCameraSCC", "0"), ("HyundaiCameraSCC", "0"),