From c959947b8f7015a46bf0746ef86854f6779dca1e Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Mon, 30 Dec 2024 14:05:24 +0100 Subject: [PATCH] [WIP] Adding support for NDv2 --- selfdrive/modeld/modeld.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 15d9921d8a..d57bf78b71 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -97,6 +97,14 @@ class ModelState: self.full_features_20Hz[-1] = outputs['hidden_state'][0, :] self.numpy_inputs['features_buffer'][:] = self.full_features_20Hz[self.full_features_20Hz_idxs] + if "desired_curvature" in outputs: + if "prev_desired_curvs" in self.numpy_inputs.keys(): + self.numpy_inputs['prev_desired_curvs'][:-1] = self.numpy_inputs['prev_desired_curvs'][1:] + self.numpy_inputs['prev_desired_curvs'][-1] = outputs['desired_curvature'][:, 0:1, None] # Reshape to (1,1,1) + if "prev_desired_curv" in self.numpy_inputs.keys(): + # First shift everything + self.numpy_inputs['prev_desired_curv'][:-ModelConstants.PREV_DESIRED_CURV_LEN] = self.numpy_inputs['prev_desired_curv'][ModelConstants.PREV_DESIRED_CURV_LEN:] + self.numpy_inputs['prev_desired_curv'][-ModelConstants.PREV_DESIRED_CURV_LEN:] = outputs['desired_curvature'][:, :1].reshape(1, -1, 1) return outputs