diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 64e4461483b73ba..2915c29947a2a2b 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -1,17 +1,16 @@ import math -from selfdrive.controls.lib.pid import LatPIDController +from selfdrive.controls.lib.pid import PIController from selfdrive.controls.lib.drive_helpers import get_steer_max from cereal import log class LatControlPID(): def __init__(self, CP): - self.pid = LatPIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), - (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), - (CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV), - k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0, - sat_limit=CP.steerLimitTimer) + self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), + (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), + k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0, + sat_limit=CP.steerLimitTimer) def reset(self): self.pid.reset() @@ -24,7 +23,8 @@ def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvatur angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo)) angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg - if CS.vEgo < 0.3 or not active or not CS.lkasEnable: + pid_log.angleError = angle_steers_des - CS.steeringAngleDeg + if CS.vEgo < 0.3 or not active: output_steer = 0.0 pid_log.active = False self.pid.reset() @@ -36,8 +36,6 @@ def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvatur # TODO: feedforward something based on lat_plan.rateSteers steer_feedforward = angle_steers_des_no_offset # offset does not contribute to resistive torque steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel) - #_c1, _c2, _c3 = [0.35189607550172824, 7.506201251644202, 69.226826411091] - #steer_feedforward *= _c1 * CS.vEgo ** 2 + _c2 * CS.vEgo + _c3 deadzone = 0.0 @@ -51,4 +49,4 @@ def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvatur pid_log.output = output_steer pid_log.saturated = bool(self.pid.saturated) - return output_steer, angle_steers_des, pid_log + return output_steer, angle_steers_des, pid_log \ No newline at end of file diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 811383f718a459a..70998f0b46619d3 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,6 +1,6 @@ from cereal import log from common.numpy_fast import clip, interp -from selfdrive.controls.lib.pid import LongPIController +from selfdrive.controls.lib.pid import PIController from selfdrive.controls.lib.drive_helpers import CONTROL_N from selfdrive.modeld.constants import T_IDXS diff --git a/selfdrive/thermald/eon_battery_manager.py b/selfdrive/thermald/eon_battery_manager.py index 23c047ae338d6f6..9f7c236b12d1024 100644 --- a/selfdrive/thermald/eon_battery_manager.py +++ b/selfdrive/thermald/eon_battery_manager.py @@ -8,21 +8,22 @@ def setEONChargingStatus(car_voltage_mV, batteryPercent) : if car_voltage_mV is None or batteryPercent is None : HARDWARE.set_battery_charging(True) return - #print( "car_voltage_mV:",car_voltage_mV) - #print( "batteryPercent:",batteryPercent) - #print( "VBATT_PAUSE_CHARGING:",VBATT_PAUSE_CHARGING) - #print("VBATT_PAUSE_CHARGING * 1e3:", VBATT_PAUSE_CHARGING * 1e3) + # print( "car_voltage_mV:",car_voltage_mV) + # print( "batteryPercent:",batteryPercent) + # print( "VBATT_PAUSE_CHARGING:",VBATT_PAUSE_CHARGING) + # print("VBATT_PAUSE_CHARGING * 1e3:", VBATT_PAUSE_CHARGING * 1e3) if HARDWARE.get_battery_charging() : - #print("log purpose : HARDWARE.get_battery_charging() True ") + # print("log purpose : HARDWARE.get_battery_charging() True ") # if batteryPercent > BATT_PERC_MAX or car_voltage_mV < VBATT_PAUSE_CHARGING * 1e3 : if batteryPercent > BATT_PERC_MAX : #or car_voltage_mV < VBATT_PAUSE_CHARGING * 1e3: - #print("log purpose : HARDWARE.set_battery_charging(False) False ") + # print("log purpose : HARDWARE.set_battery_charging(False) False ") HARDWARE.set_battery_charging(False) return else : - #print("log purpose : HARDWARE.get_battery_charging() False ") + # print("log purpose : HARDWARE.get_battery_charging() False ") #if batteryPercent < BATT_PERC_MIN and car_voltage_mV > VBATT_PAUSE_CHARGING * 1e3: if batteryPercent < BATT_PERC_MIN :#and car_voltage_mV > VBATT_PAUSE_CHARGING * 1e3: #print("log purpose : HARDWARE.set_battery_charging(True) True ") HARDWARE.set_battery_charging(True) return + diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 555722d4e3093bf..0d5a84ef1018653 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -409,11 +409,12 @@ def thermald_thread(): msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(pandaState, off_ts) # Set EON charging disable - # based on kegman, й͸ , ̿ ͸ ۼƼ, + # based on kegman, ���� ���й��͸��� ����, �̿� ���͸� �ۼ�Ƽ��, if EON: from selfdrive.thermald.eon_battery_manager import setEONChargingStatus setEONChargingStatus(power_monitor.car_voltage_mV, msg.deviceState.batteryPercent) + # Check if we need to shut down if power_monitor.should_shutdown(pandaState, off_ts, started_seen): cloudlog.info(f"shutting device down, offroad since {off_ts}") diff --git a/selfdrive/ui/qt/sidebar.cc b/selfdrive/ui/qt/sidebar.cc index b786fe8c12cda18..5e419e56ddab2e2 100644 --- a/selfdrive/ui/qt/sidebar.cc +++ b/selfdrive/ui/qt/sidebar.cc @@ -92,7 +92,7 @@ void Sidebar::updateState(const UIState &s) { } else { network_str = net_type; } - temp_val = deviceState.getAmbientTempC(); +// temp_val = deviceState.getAmbientTempC(); batt_perc = deviceState.getBatteryPercent(); setProperty("tempVal", deviceState.getAmbientTempC()); @@ -123,15 +123,16 @@ void Sidebar::paintEvent(QPaintEvent *event) { configFont(p, "Open Sans", 35, "Regular"); p.setPen(QColor(0xff, 0xff, 0xff)); - const QRect r = QRect(50, 247, 100, 50); - p.drawText(r, Qt::AlignCenter, net_type); + const QRect r = QRect(25, 247, 250, 50); + + p.drawText(r, Qt::AlignCenter, network_str); // metrics -// drawMetric(p, "TEMP", temp_status.first, temp_status.second, 338); -// drawMetric(p, panda_status.first, "", panda_status.second, 518); -// drawMetric(p, connect_status.first, "", connect_status.second, 676); +// drawMetric(p, temp_status.first, temp_status.second, 338); +// drawMetric(p, panda_status.first, panda_status.second, 518); +// drawMetric(p, connect_status.first, connect_status.second, 676); QString batt_perc_qstring = QString("BATT: %1 %2").arg(batt_perc).arg("%"); - drawMetric(p, batt_perc_qstring, QString("%1°C").arg(temp_val), temp_status.second, 338); - drawMetric(p, panda_status.first, "", panda_status.second, 518); - drawMetric(p, "네트워크\n" + connect_status.first, "", connect_status.second, 676); + drawMetric(p, batt_perc_qstring +"\n"+ QString("%1°C").arg((double)temp_val,4,'f',1), temp_status.second, 338); + drawMetric(p, panda_status.first, panda_status.second, 518); + drawMetric(p, "네트워크\n" + connect_status.first, connect_status.second, 676); }