diff --git a/RELEASES.md b/RELEASES.md index ad51b7af3f..bf89b667fa 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -6,8 +6,9 @@ Version 0.8.17 (2022-11-21) * Trained in new reprojective simulator * Trained in 36hrs from scratch, compared to one week for previous releases * Training now simulates both lateral and longitudinal behavior, which allows openpilot to slow down for turns, stop at traffic lights, and more in experimental mode -* New driver monitoring model - * New end-to-end distracted trigger +* Driver monitoring updates + * New bigger model with added end-to-end distracted trigger + * Reduced false positives during driver calibration * Experimental driving mode * End-to-end longitudinal control * Stops for traffic lights and stop signs diff --git a/panda b/panda index 281eb7731b..d573111268 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 281eb7731b4338fef049977593fdf3315adf09e9 +Subproject commit d57311126860c9a87edf5b7b9f81a2a038866a26 diff --git a/selfdrive/boardd/panda_comms.h b/selfdrive/boardd/panda_comms.h index aef7b41d07..f42eadc5b2 100644 --- a/selfdrive/boardd/panda_comms.h +++ b/selfdrive/boardd/panda_comms.h @@ -34,7 +34,7 @@ public: virtual int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT) = 0; protected: - std::mutex hw_lock; + std::recursive_mutex hw_lock; }; class PandaUsbHandle : public PandaCommsHandle { @@ -74,6 +74,7 @@ private: uint8_t rx_buf[SPI_BUF_SIZE]; int wait_for_ack(spi_ioc_transfer &transfer, uint8_t ack); + int bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len); int spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len); int spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len); }; diff --git a/selfdrive/boardd/spi.cc b/selfdrive/boardd/spi.cc index 3969b313f0..2803f58db0 100644 --- a/selfdrive/boardd/spi.cc +++ b/selfdrive/boardd/spi.cc @@ -2,6 +2,7 @@ #include #include +#include #include #include "common/util.h" @@ -99,13 +100,45 @@ int PandaSpiHandle::control_read(uint8_t request, uint16_t param1, uint16_t para } int PandaSpiHandle::bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { - return 0; + return bulk_transfer(endpoint, data, length, NULL, 0); } - int PandaSpiHandle::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { - return 0; + return bulk_transfer(endpoint, NULL, 0, data, length); +} + +int PandaSpiHandle::bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len) { + std::lock_guard lk(hw_lock); + + const int xfer_size = 0x40; + + int ret = 0; + uint16_t length = (tx_data != NULL) ? tx_len : rx_len; + for (int i = 0; i < (int)std::ceil((float)length / xfer_size); i++) { + int d; + if (tx_data != NULL) { + int len = std::min(xfer_size, tx_len - (xfer_size * i)); + d = spi_transfer_retry(endpoint, tx_data + (xfer_size * i), len, NULL, 0); + } else { + d = spi_transfer_retry(endpoint, NULL, 0, rx_data + (xfer_size * i), xfer_size); + } + + if (d < 0) { + LOGE("SPI: bulk transfer failed with %d", d); + comms_healthy = false; + return -1; + } + + ret += d; + if ((rx_data != NULL) && d < xfer_size) { + break; + } + } + + return ret; } + + std::vector PandaSpiHandle::list() { // TODO: list all pandas available over SPI return {}; @@ -130,15 +163,15 @@ bool check_checksum(uint8_t *data, int data_len) { int PandaSpiHandle::spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len) { - int err; + int ret; std::lock_guard lk(hw_lock); do { // TODO: handle error - err = spi_transfer(endpoint, tx_data, tx_len, rx_data, max_rx_len); - } while (err < 0 && connected && !PANDA_NO_RETRY); + ret = spi_transfer(endpoint, tx_data, tx_len, rx_data, max_rx_len); + } while (ret < 0 && connected && !PANDA_NO_RETRY); - return err; + return ret; } int PandaSpiHandle::wait_for_ack(spi_ioc_transfer &transfer, uint8_t ack) { @@ -153,7 +186,7 @@ int PandaSpiHandle::wait_for_ack(spi_ioc_transfer &transfer, uint8_t ack) { if (rx_buf[0] == ack) { break; } else if (rx_buf[0] == SPI_NACK) { - LOGW("SPI: got header NACK"); + LOGW("SPI: got NACK"); return -1; } } diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 44426620e9..a761cceecb 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -597,6 +597,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { EventName.buttonCancel: { ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), + ET.NO_ENTRY: NoEntryAlert("Cancel Pressed"), }, EventName.brakeHold: { diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index cae378453c..def0a1208a 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -69,7 +69,8 @@ class LongitudinalPlanner: e2e = self.params.get_bool('ExperimentalMode') and self.CP.openpilotLongitudinalControl self.mpc.mode = 'blended' if e2e else 'acc' - def parse_model(self, model_msg, model_error): + @staticmethod + def parse_model(model_msg, model_error): if (len(model_msg.position.x) == 33 and len(model_msg.velocity.x) == 33 and len(model_msg.acceleration.x) == 33): @@ -107,6 +108,7 @@ class LongitudinalPlanner: accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP) else: + accel_limits = [MIN_ACCEL, MAX_ACCEL] accel_limits_turns = [MIN_ACCEL, MAX_ACCEL] if reset_state: diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index dad5d89844..071eaada12 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -17,6 +17,7 @@ class Maneuver(): self.only_lead2 = kwargs.get("only_lead2", False) self.only_radar = kwargs.get("only_radar", False) self.ensure_start = kwargs.get("ensure_start", False) + self.enabled = kwargs.get("enabled", True) self.duration = duration self.title = title @@ -26,23 +27,24 @@ class Maneuver(): lead_relevancy=self.lead_relevancy, speed=self.speed, distance_lead=self.distance_lead, + enabled=self.enabled, only_lead2=self.only_lead2, only_radar=self.only_radar, ) valid = True logs = [] - while plant.current_time() < self.duration: - speed_lead = np.interp(plant.current_time(), self.breakpoints, self.speed_lead_values) - prob = np.interp(plant.current_time(), self.breakpoints, self.prob_lead_values) - cruise = np.interp(plant.current_time(), self.breakpoints, self.cruise_values) + while plant.current_time < self.duration: + speed_lead = np.interp(plant.current_time, self.breakpoints, self.speed_lead_values) + prob = np.interp(plant.current_time, self.breakpoints, self.prob_lead_values) + cruise = np.interp(plant.current_time, self.breakpoints, self.cruise_values) log = plant.step(speed_lead, prob, cruise) d_rel = log['distance_lead'] - log['distance'] if self.lead_relevancy else 200. v_rel = speed_lead - log['speed'] if self.lead_relevancy else 0. log['d_rel'] = d_rel log['v_rel'] = v_rel - logs.append(np.array([plant.current_time(), + logs.append(np.array([plant.current_time, log['distance'], log['distance_lead'], log['speed'], diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index e81510e9ba..c3af1eee03 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -10,11 +10,12 @@ from selfdrive.modeld.constants import T_IDXS from selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU -class Plant(): + +class Plant: messaging_initialized = False def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0, - only_lead2=False, only_radar=False): + enabled=True, only_lead2=False, only_radar=False): self.rate = 1. / DT_MDL if not Plant.messaging_initialized: @@ -32,10 +33,11 @@ class Plant(): self.speeds = [] # lead car - self.distance_lead = distance_lead self.lead_relevancy = lead_relevancy - self.only_lead2=only_lead2 - self.only_radar=only_radar + self.distance_lead = distance_lead + self.enabled = enabled + self.only_lead2 = only_lead2 + self.only_radar = only_radar self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0) self.ts = 1. / self.rate @@ -47,6 +49,7 @@ class Plant(): self.planner = LongitudinalPlanner(CarInterface.get_params(CAR.CIVIC), init_v=self.speed) + @property def current_time(self): return float(self.rk.frame) / self.rate @@ -104,9 +107,7 @@ class Plant(): acceleration.x = [float(x) for x in np.zeros_like(T_IDXS)] model.modelV2.acceleration = acceleration - - - control.controlsState.longControlState = LongCtrlState.pid + control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off control.controlsState.vCruise = float(v_cruise * 3.6) car_state.carState.vEgo = float(self.speed) car_state.carState.standstill = self.speed < 0.01 @@ -141,7 +142,7 @@ class Plant(): # print at 5hz if (self.rk.frame % (self.rate // 5)) == 0: print("%2.2f sec %6.2f m %6.2f m/s %6.2f m/s2 lead_rel: %6.2f m %6.2f m/s" - % (self.current_time(), self.distance, self.speed, self.acceleration, d_rel, v_rel)) + % (self.current_time, self.distance, self.speed, self.acceleration, d_rel, v_rel)) # ******** update prevs ******** diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 7cc95b104a..e859952445 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -10,7 +10,7 @@ from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver # TODO: make new FCW tests maneuvers = [ Maneuver( - 'approach stopped car at 20m/s, initial distance: 120m', + 'approach stopped car at 25m/s, initial distance: 120m', duration=20., initial_speed=25., lead_relevancy=True, @@ -118,6 +118,13 @@ maneuvers = [ breakpoints=[1., 10., 15.], ensure_start=True, ), + Maneuver( + 'cruising at 25 m/s while disabled', + duration=20., + initial_speed=25., + lead_relevancy=False, + enabled=False, + ), ] diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index e3c5390c3b..2ff423a84a 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -9e37e8ee8013515cfd75cf352a1a2b9aa447c441 +b5c833a8f5b3e6202a52746fc16809c7b649d591