mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-21 11:23:47 +08:00
Merge remote-tracking branch 'sunnypilot/sunnypilot/mads-disengage-on-brake' into mads-tesla
# Conflicts: # opendbc_repo # selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc # selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h # sunnypilot/mads/helpers.py # sunnypilot/mads/mads.py
This commit is contained in:
+6
-5
@@ -63,7 +63,7 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
|
||||
type @0 :Type;
|
||||
artifact @1 :Artifact; # Main artifact
|
||||
metadata @2 :Artifact; # Metadata artifact
|
||||
|
||||
|
||||
enum Type {
|
||||
supercombo @0;
|
||||
navigation @1;
|
||||
@@ -144,6 +144,7 @@ struct OnroadEventSP @0xda96579883444c35 {
|
||||
hyundaiRadarTracksConfirmed @13;
|
||||
experimentalModeSwitched @14;
|
||||
wrongCarModeAlertOnly @15;
|
||||
pedalPressedAlertOnly @16;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -176,14 +177,14 @@ struct BackupManagerSP @0xf98d843bfd7004a3 {
|
||||
lastError @4 :Text;
|
||||
currentBackup @5 :BackupInfo;
|
||||
backupHistory @6 :List(BackupInfo);
|
||||
|
||||
|
||||
enum Status {
|
||||
idle @0;
|
||||
inProgress @1;
|
||||
completed @2;
|
||||
failed @3;
|
||||
}
|
||||
|
||||
|
||||
struct Version {
|
||||
major @0 :UInt16;
|
||||
minor @1 :UInt16;
|
||||
@@ -191,13 +192,13 @@ struct BackupManagerSP @0xf98d843bfd7004a3 {
|
||||
build @3 :UInt16;
|
||||
branch @4 :Text;
|
||||
}
|
||||
|
||||
|
||||
struct MetadataEntry {
|
||||
key @0 :Text;
|
||||
value @1 :Text;
|
||||
tags @2 :List(Text);
|
||||
}
|
||||
|
||||
|
||||
struct BackupInfo {
|
||||
deviceId @0 :Text;
|
||||
version @1 :UInt32;
|
||||
|
||||
+1
-1
Submodule opendbc_repo updated: 7fa1ec698e...0d0d3d4463
@@ -2,7 +2,7 @@
|
||||
import math
|
||||
from typing import SupportsFloat
|
||||
|
||||
from cereal import car, log, custom
|
||||
from cereal import car, log
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.params import Params
|
||||
@@ -19,6 +19,8 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongControl
|
||||
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.controls.controlsd_ext import ControlsExt
|
||||
|
||||
State = log.SelfdriveState.OpenpilotState
|
||||
LaneChangeState = log.LaneChangeState
|
||||
LaneChangeDirection = log.LaneChangeDirection
|
||||
@@ -26,24 +28,23 @@ LaneChangeDirection = log.LaneChangeDirection
|
||||
ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys())
|
||||
|
||||
|
||||
class Controls:
|
||||
class Controls(ControlsExt):
|
||||
def __init__(self) -> None:
|
||||
self.params = Params()
|
||||
cloudlog.info("controlsd is waiting for CarParams")
|
||||
self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams)
|
||||
cloudlog.info("controlsd got CarParams")
|
||||
|
||||
cloudlog.info("controlsd is waiting for CarParamsSP")
|
||||
self.CP_SP = messaging.log_from_bytes(self.params.get("CarParamsSP", block=True), custom.CarParamsSP)
|
||||
cloudlog.info("controlsd got CarParamsSP")
|
||||
# Initialize sunnypilot controlsd extension
|
||||
ControlsExt.__init__(self, self.params)
|
||||
|
||||
self.CI = interfaces[self.CP.carFingerprint](self.CP, self.CP_SP)
|
||||
|
||||
self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
|
||||
'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
|
||||
'driverMonitoringState', 'onroadEvents', 'driverAssistance'] + ['selfdriveStateSP'],
|
||||
'driverMonitoringState', 'onroadEvents', 'driverAssistance'] + self.sm_services_ext,
|
||||
poll='selfdriveState')
|
||||
self.pm = messaging.PubMaster(['carControl', 'controlsState'] + ['carControlSP'])
|
||||
self.pm = messaging.PubMaster(['carControl', 'controlsState'] + self.pm_services_ext)
|
||||
|
||||
self.steer_limited_by_controls = False
|
||||
self.curvature = 0.0
|
||||
@@ -100,11 +101,8 @@ class Controls:
|
||||
# Check which actuators can be enabled
|
||||
standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, 0.3) or CS.standstill
|
||||
|
||||
ss_sp = self.sm['selfdriveStateSP']
|
||||
if ss_sp.mads.available:
|
||||
_lat_active = ss_sp.mads.active
|
||||
else:
|
||||
_lat_active = self.sm['selfdriveState'].active
|
||||
# Get which state to use for active lateral control
|
||||
_lat_active = self.get_lat_active(self.sm)
|
||||
|
||||
CC.latActive = _lat_active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
|
||||
(not standstill or self.CP.steerAtStandstill)
|
||||
@@ -148,12 +146,9 @@ class Controls:
|
||||
cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}")
|
||||
setattr(actuators, p, 0.0)
|
||||
|
||||
CC_SP = custom.CarControlSP.new_message()
|
||||
CC_SP.mads = ss_sp.mads
|
||||
return CC, lac_log
|
||||
|
||||
return CC, CC_SP, lac_log
|
||||
|
||||
def publish(self, CC, CC_SP, lac_log):
|
||||
def publish(self, CC, lac_log):
|
||||
CS = self.sm['carState']
|
||||
|
||||
# Orientation and angle rates can be useful for carcontroller
|
||||
@@ -227,18 +222,13 @@ class Controls:
|
||||
cc_send.carControl = CC
|
||||
self.pm.send('carControl', cc_send)
|
||||
|
||||
# carControlSP
|
||||
cc_sp_send = messaging.new_message('carControlSP')
|
||||
cc_sp_send.valid = CS.canValid
|
||||
cc_sp_send.carControlSP = CC_SP
|
||||
self.pm.send('carControlSP', cc_sp_send)
|
||||
|
||||
def run(self):
|
||||
rk = Ratekeeper(100, print_delay_threshold=None)
|
||||
while True:
|
||||
self.update()
|
||||
CC, CC_SP, lac_log = self.state_control()
|
||||
self.publish(CC, CC_SP, lac_log)
|
||||
CC, lac_log = self.state_control()
|
||||
self.publish(CC, lac_log)
|
||||
self.run_ext(self.sm, self.pm)
|
||||
rk.monitor_time()
|
||||
|
||||
|
||||
|
||||
@@ -71,6 +71,13 @@ SUPPORTED_FW_VERSIONS = {
|
||||
b"DLhe SCC FHCUP 1.00 1.02 99110-L7000 \x01 \x102 ": ConfigValues(
|
||||
default_config=b"\x00\x00\x00\x01\x00\x00",
|
||||
tracks_enabled=b"\x00\x00\x00\x01\x00\x01"),
|
||||
# 2022 Niro EV
|
||||
b"DEev SCC F-CUP 1.00 1.00 99110-Q4600\x01\x42 ": ConfigValues(
|
||||
default_config=b"\x00\x00\x00\x01\x00\x00",
|
||||
tracks_enabled=b"\x00\x00\x00\x01\x00\x01"),
|
||||
b"DEev SCC F-CUP 1.00 1.00 99110-Q4600 \x07\x03\t% ": ConfigValues(
|
||||
default_config=b"\x00\x00\x00\x01\x00\x00",
|
||||
tracks_enabled=b"\x00\x00\x00\x01\x00\x01"),
|
||||
}
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
@@ -40,18 +40,18 @@ MadsSettings::MadsSettings(QWidget *parent) : QWidget(parent) {
|
||||
list->addItem(madsUnifiedEngagementModeToggle);
|
||||
|
||||
// Steering Mode On Brake
|
||||
std::vector<QString> lateral_on_brake_texts{tr("Remain Active"), tr("Pause Steering"), tr("Disengage")};
|
||||
madsSteeringMode = new ButtonParamControl(
|
||||
"MadsSteeringMode",
|
||||
tr("Steering Mode on Brake Pedal"),
|
||||
tr("Choose how Automatic Lane Centering (ALC) behaves after the brake pedal is manually pressed in sunnypilot.\n\n"
|
||||
"Remain Active: ALC will remain active even after the brake pedal is pressed.\n"
|
||||
"Pause Steering: ALC will be paused when the brake pedal is manually pressed."),
|
||||
"Disengage: ALC will be disengaged after the brake pedal is pressed.\n"
|
||||
"",
|
||||
lateral_on_brake_texts,
|
||||
"",
|
||||
madsSteeringModeTexts(),
|
||||
500);
|
||||
QObject::connect(madsSteeringMode, &ButtonParamControl::buttonToggled, [=] {
|
||||
updateToggles(offroad);
|
||||
});
|
||||
list->addItem(madsSteeringMode);
|
||||
madsSteeringMode->showDescription();
|
||||
|
||||
QObject::connect(uiState(), &UIState::offroadTransition, this, &MadsSettings::updateToggles);
|
||||
|
||||
@@ -63,7 +63,20 @@ void MadsSettings::showEvent(QShowEvent *event) {
|
||||
}
|
||||
|
||||
void MadsSettings::updateToggles(bool _offroad) {
|
||||
auto mads_steering_mode_param = std::atoi(params.get("MadsSteeringMode").c_str());
|
||||
|
||||
MadsSteeringMode steering_mode;
|
||||
if (mads_steering_mode_param == static_cast<int>(MadsSteeringMode::REMAIN_ACTIVE)) {
|
||||
steering_mode = MadsSteeringMode::REMAIN_ACTIVE;
|
||||
} else if (mads_steering_mode_param == static_cast<int>(MadsSteeringMode::PAUSE)) {
|
||||
steering_mode = MadsSteeringMode::PAUSE;
|
||||
} else {
|
||||
steering_mode = MadsSteeringMode::DISENGAGE;
|
||||
}
|
||||
|
||||
madsSteeringMode->setEnabled(_offroad);
|
||||
madsSteeringMode->setDescription(madsSteeringModeDescription(steering_mode));
|
||||
madsSteeringMode->showDescription();
|
||||
|
||||
offroad = _offroad;
|
||||
}
|
||||
|
||||
@@ -12,6 +12,18 @@
|
||||
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
|
||||
#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
|
||||
|
||||
enum class MadsSteeringMode {
|
||||
REMAIN_ACTIVE = 0,
|
||||
PAUSE = 1,
|
||||
DISENGAGE = 2,
|
||||
};
|
||||
|
||||
struct MadsSteeringModeOption {
|
||||
MadsSteeringMode mode;
|
||||
QString display_text;
|
||||
QString description;
|
||||
};
|
||||
|
||||
class MadsSettings : public QWidget {
|
||||
Q_OBJECT
|
||||
|
||||
@@ -33,4 +45,36 @@ private:
|
||||
ParamControl *madsMainCruiseToggle;
|
||||
ParamControl *madsUnifiedEngagementModeToggle;
|
||||
ButtonParamControl *madsSteeringMode;
|
||||
|
||||
static const std::vector<MadsSteeringModeOption> &madsSteeringModeOptions() {
|
||||
static const std::vector<MadsSteeringModeOption> options = {
|
||||
{MadsSteeringMode::REMAIN_ACTIVE, tr("Remain Active"), tr("Remain Active: ALC will remain active when the brake pedal is pressed.")},
|
||||
{MadsSteeringMode::PAUSE, tr("Pause"), tr("Pause: ALC will pause steering when the brake pedal is pressed.")},
|
||||
{MadsSteeringMode::DISENGAGE, tr("Disengage"), tr("Disengage: ALC will disengage when the brake pedal is pressed.")},
|
||||
};
|
||||
return options;
|
||||
}
|
||||
|
||||
static std::vector<QString> madsSteeringModeTexts() {
|
||||
std::vector<QString> texts;
|
||||
for (const auto& option : madsSteeringModeOptions()) {
|
||||
texts.push_back(option.display_text);
|
||||
}
|
||||
return texts;
|
||||
}
|
||||
|
||||
static QString madsSteeringModeDescription(const MadsSteeringMode mode) {
|
||||
QString base_desc = tr("Choose how Automatic Lane Centering (ALC) behaves after the brake pedal is manually pressed in sunnypilot.");
|
||||
QString result = base_desc + "<br><br>";
|
||||
|
||||
for (const auto& option : madsSteeringModeOptions()) {
|
||||
QString desc = option.description;
|
||||
if (option.mode == mode) {
|
||||
desc = "<font color='white'><b>" + desc + "</b></font>";
|
||||
}
|
||||
result += desc + "<br>";
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
};
|
||||
|
||||
+86
-45
@@ -9,6 +9,7 @@ from cereal import log, custom
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.hyundai.values import HyundaiFlags
|
||||
from opendbc.safety import ALTERNATIVE_EXPERIENCE
|
||||
from openpilot.sunnypilot.mads.helpers import MadsSteeringModeOnBrake, read_steering_mode_param
|
||||
from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT
|
||||
|
||||
@@ -25,6 +26,7 @@ IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
|
||||
|
||||
class ModularAssistiveDrivingSystem:
|
||||
def __init__(self, selfdrive):
|
||||
self.CP = selfdrive.CP
|
||||
self.params = selfdrive.params
|
||||
|
||||
self.enabled = False
|
||||
@@ -37,9 +39,10 @@ class ModularAssistiveDrivingSystem:
|
||||
self.state_machine = StateMachine(self)
|
||||
self.events = self.selfdrive.events
|
||||
self.events_sp = self.selfdrive.events_sp
|
||||
self.disengage_on_accelerator = not self.CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
|
||||
|
||||
if self.selfdrive.CP.brand == "hyundai":
|
||||
if self.selfdrive.CP.flags & (HyundaiFlags.HAS_LDA_BUTTON | HyundaiFlags.CANFD):
|
||||
if self.CP.brand == "hyundai":
|
||||
if self.CP.flags & (HyundaiFlags.HAS_LDA_BUTTON | HyundaiFlags.CANFD):
|
||||
self.allow_always = True
|
||||
|
||||
if self.selfdrive.CP.brand in ("rivian", "tesla"):
|
||||
@@ -48,51 +51,80 @@ class ModularAssistiveDrivingSystem:
|
||||
# read params on init
|
||||
self.enabled_toggle = self.params.get_bool("Mads")
|
||||
self.main_enabled_toggle = self.params.get_bool("MadsMainCruiseAllowed") and not self.no_main_cruise
|
||||
self.steering_mode_on_brake = read_steering_mode_param(self.selfdrive.CP, self.params)
|
||||
self.steering_mode_on_brake = read_steering_mode_param(self.CP, self.params)
|
||||
self.unified_engagement_mode = self.params.get_bool("MadsUnifiedEngagementMode")
|
||||
|
||||
def read_params(self):
|
||||
self.main_enabled_toggle = self.params.get_bool("MadsMainCruiseAllowed") and not self.no_main_cruise
|
||||
self.unified_engagement_mode = self.params.get_bool("MadsUnifiedEngagementMode")
|
||||
|
||||
def pedal_pressed_non_gas_pressed(self, CS: structs.CarState) -> bool:
|
||||
if self.events.has(EventName.pedalPressed) and not (CS.gasPressed and not self.selfdrive.CS_prev.gasPressed and self.disengage_on_accelerator):
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
def should_silent_lkas_enable(self, CS: structs.CarState) -> bool:
|
||||
if self.steering_mode_on_brake == MadsSteeringModeOnBrake.PAUSE and self.pedal_pressed_non_gas_pressed(CS):
|
||||
return False
|
||||
|
||||
if self.events_sp.contains_in_list(GEARS_ALLOW_PAUSED_SILENT):
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
def block_unified_engagement_mode(self) -> bool:
|
||||
# UEM disabled
|
||||
if not self.unified_engagement_mode:
|
||||
return True
|
||||
|
||||
if self.enabled:
|
||||
return True
|
||||
|
||||
if self.selfdrive.enabled and self.selfdrive.enabled_prev:
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
def get_wrong_car_mode(self, alert_only: bool) -> None:
|
||||
if alert_only:
|
||||
if self.events.has(EventName.wrongCarMode):
|
||||
self.replace_event(EventName.wrongCarMode, EventNameSP.wrongCarModeAlertOnly)
|
||||
else:
|
||||
self.events.remove(EventName.wrongCarMode)
|
||||
|
||||
def transition_paused_state(self):
|
||||
if self.state_machine.state != State.paused:
|
||||
self.events_sp.add(EventNameSP.silentLkasDisable)
|
||||
|
||||
def replace_event(self, old_event: int, new_event: int):
|
||||
self.events.remove(old_event)
|
||||
self.events_sp.add(new_event)
|
||||
|
||||
def update_events(self, CS: structs.CarState):
|
||||
def update_unified_engagement_mode():
|
||||
uem_blocked = self.enabled or (self.selfdrive.enabled and self.selfdrive.enabled_prev)
|
||||
if (self.unified_engagement_mode and uem_blocked) or not self.unified_engagement_mode:
|
||||
self.events.remove(EventName.pcmEnable)
|
||||
self.events.remove(EventName.buttonEnable)
|
||||
|
||||
def transition_paused_state():
|
||||
if self.state_machine.state != State.paused:
|
||||
self.events_sp.add(EventNameSP.silentLkasDisable)
|
||||
|
||||
def replace_event(old_event: int, new_event: int):
|
||||
self.events.remove(old_event)
|
||||
self.events_sp.add(new_event)
|
||||
|
||||
if not self.selfdrive.enabled and self.enabled:
|
||||
if self.events.has(EventName.doorOpen):
|
||||
replace_event(EventName.doorOpen, EventNameSP.silentDoorOpen)
|
||||
transition_paused_state()
|
||||
self.replace_event(EventName.doorOpen, EventNameSP.silentDoorOpen)
|
||||
self.transition_paused_state()
|
||||
if self.events.has(EventName.seatbeltNotLatched):
|
||||
replace_event(EventName.seatbeltNotLatched, EventNameSP.silentSeatbeltNotLatched)
|
||||
transition_paused_state()
|
||||
self.replace_event(EventName.seatbeltNotLatched, EventNameSP.silentSeatbeltNotLatched)
|
||||
self.transition_paused_state()
|
||||
if self.events.has(EventName.wrongGear) and (CS.vEgo < 2.5 or CS.gearShifter == GearShifter.reverse):
|
||||
replace_event(EventName.wrongGear, EventNameSP.silentWrongGear)
|
||||
transition_paused_state()
|
||||
self.replace_event(EventName.wrongGear, EventNameSP.silentWrongGear)
|
||||
self.transition_paused_state()
|
||||
if self.events.has(EventName.reverseGear):
|
||||
replace_event(EventName.reverseGear, EventNameSP.silentReverseGear)
|
||||
transition_paused_state()
|
||||
self.replace_event(EventName.reverseGear, EventNameSP.silentReverseGear)
|
||||
self.transition_paused_state()
|
||||
if self.events.has(EventName.brakeHold):
|
||||
replace_event(EventName.brakeHold, EventNameSP.silentBrakeHold)
|
||||
transition_paused_state()
|
||||
self.replace_event(EventName.brakeHold, EventNameSP.silentBrakeHold)
|
||||
self.transition_paused_state()
|
||||
if self.events.has(EventName.parkBrake):
|
||||
replace_event(EventName.parkBrake, EventNameSP.silentParkBrake)
|
||||
transition_paused_state()
|
||||
self.replace_event(EventName.parkBrake, EventNameSP.silentParkBrake)
|
||||
self.transition_paused_state()
|
||||
|
||||
if self.steering_mode_on_brake == MadsSteeringModeOnBrake.PAUSE:
|
||||
if CS.brakePressed:
|
||||
transition_paused_state()
|
||||
if self.pedal_pressed_non_gas_pressed(CS):
|
||||
self.transition_paused_state()
|
||||
|
||||
self.events.remove(EventName.preEnableStandstill)
|
||||
self.events.remove(EventName.belowEngageSpeed)
|
||||
@@ -100,8 +132,19 @@ class ModularAssistiveDrivingSystem:
|
||||
self.events.remove(EventName.cruiseDisabled)
|
||||
self.events.remove(EventName.manualRestart)
|
||||
|
||||
if self.events.has(EventName.pcmEnable) or self.events.has(EventName.buttonEnable):
|
||||
update_unified_engagement_mode()
|
||||
selfdrive_enable_events = self.events.has(EventName.pcmEnable) or self.events.has(EventName.buttonEnable)
|
||||
set_speed_btns_enable = any(be.type in SET_SPEED_BUTTONS for be in CS.buttonEvents)
|
||||
|
||||
# wrongCarMode alert only or actively block control
|
||||
self.get_wrong_car_mode(selfdrive_enable_events or set_speed_btns_enable)
|
||||
|
||||
if selfdrive_enable_events:
|
||||
if self.pedal_pressed_non_gas_pressed(CS):
|
||||
self.events_sp.add(EventNameSP.pedalPressedAlertOnly)
|
||||
|
||||
if self.block_unified_engagement_mode():
|
||||
self.events.remove(EventName.pcmEnable)
|
||||
self.events.remove(EventName.buttonEnable)
|
||||
else:
|
||||
if self.main_enabled_toggle:
|
||||
if CS.cruiseState.available and not self.selfdrive.CS_prev.cruiseState.available:
|
||||
@@ -126,13 +169,16 @@ class ModularAssistiveDrivingSystem:
|
||||
self.events_sp.add(EventNameSP.lkasDisable)
|
||||
|
||||
if self.steering_mode_on_brake == MadsSteeringModeOnBrake.DISENGAGE:
|
||||
# Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0
|
||||
if (CS.brakePressed and (not self.selfdrive.CS_prev.brakePressed or not CS.standstill)) or \
|
||||
(CS.regenBraking and (not self.selfdrive.CS_prev.regenBraking or not CS.standstill)):
|
||||
self.events_sp.add(EventNameSP.lkasDisable)
|
||||
if self.pedal_pressed_non_gas_pressed(CS):
|
||||
if self.enabled:
|
||||
self.events_sp.add(EventNameSP.lkasDisable)
|
||||
else:
|
||||
# block lkasEnable if being sent, then send pedalPressedAlertOnly event
|
||||
if self.events_sp.contains(EventNameSP.lkasEnable):
|
||||
self.events_sp.remove(EventNameSP.lkasEnable)
|
||||
self.events_sp.add(EventNameSP.pedalPressedAlertOnly)
|
||||
|
||||
if not (self.steering_mode_on_brake == MadsSteeringModeOnBrake.PAUSE and CS.brakePressed) and \
|
||||
not self.events_sp.contains_in_list(GEARS_ALLOW_PAUSED_SILENT):
|
||||
if self.should_silent_lkas_enable(CS):
|
||||
if self.state_machine.state == State.paused:
|
||||
self.events_sp.add(EventNameSP.silentLkasEnable)
|
||||
|
||||
@@ -140,11 +186,6 @@ class ModularAssistiveDrivingSystem:
|
||||
self.events.remove(EventName.buttonCancel)
|
||||
self.events.remove(EventName.pedalPressed)
|
||||
self.events.remove(EventName.wrongCruiseMode)
|
||||
if any(be.type in SET_SPEED_BUTTONS for be in CS.buttonEvents):
|
||||
if self.events.has(EventName.wrongCarMode):
|
||||
replace_event(EventName.wrongCarMode, EventNameSP.wrongCarModeAlertOnly)
|
||||
else:
|
||||
self.events.remove(EventName.wrongCarMode)
|
||||
|
||||
def update(self, CS: structs.CarState):
|
||||
if not self.enabled_toggle:
|
||||
@@ -152,7 +193,7 @@ class ModularAssistiveDrivingSystem:
|
||||
|
||||
self.update_events(CS)
|
||||
|
||||
if not self.selfdrive.CP.passive and self.selfdrive.initialized:
|
||||
if not self.CP.passive and self.selfdrive.initialized:
|
||||
self.enabled, self.active = self.state_machine.update()
|
||||
|
||||
# Copy of previous SelfdriveD states for MADS events handling
|
||||
|
||||
@@ -0,0 +1,53 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal import custom
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
|
||||
class ControlsExt:
|
||||
def __init__(self, params: Params):
|
||||
cloudlog.info("controlsd_ext is waiting for CarParamsSP")
|
||||
self.CP_SP = messaging.log_from_bytes(params.get("CarParamsSP", block=True), custom.CarParamsSP)
|
||||
cloudlog.info("controlsd_ext got CarParamsSP")
|
||||
|
||||
self.sm_services_ext = ['selfdriveStateSP']
|
||||
self.pm_services_ext = ['carControlSP']
|
||||
|
||||
@staticmethod
|
||||
def get_lat_active(sm: messaging.SubMaster) -> bool:
|
||||
ss_sp = sm['selfdriveStateSP']
|
||||
|
||||
if ss_sp.mads.available:
|
||||
return bool(ss_sp.mads.active)
|
||||
|
||||
# MADS not available, use stock state to engage
|
||||
return bool(sm['selfdriveState'].active)
|
||||
|
||||
@staticmethod
|
||||
def state_control_ext(sm: messaging.SubMaster) -> custom.CarControlSP:
|
||||
CC_SP = custom.CarControlSP.new_message()
|
||||
|
||||
# MADS state
|
||||
CC_SP.mads = sm['selfdriveStateSP'].mads
|
||||
|
||||
return CC_SP
|
||||
|
||||
@staticmethod
|
||||
def publish_ext(CC_SP: custom.CarControlSP, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
|
||||
cc_sp_send = messaging.new_message('carControlSP')
|
||||
cc_sp_send.valid = sm['carState'].canValid
|
||||
cc_sp_send.carControlSP = CC_SP
|
||||
|
||||
pm.send('carControlSP', cc_sp_send)
|
||||
|
||||
def run_ext(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
|
||||
CC_SP = self.state_control_ext(sm)
|
||||
self.publish_ext(CC_SP, sm, pm)
|
||||
@@ -134,4 +134,8 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
|
||||
ET.WARNING: wrong_car_mode_alert,
|
||||
},
|
||||
|
||||
EventNameSP.pedalPressedAlertOnly: {
|
||||
ET.WARNING: NoEntryAlert("Pedal Pressed")
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user