diff --git a/cereal b/cereal index 7a3b02dabe..6fe062eca0 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 7a3b02dabe9bc0193e741d97ccfeed3cb0953d56 +Subproject commit 6fe062eca0c576cd0629cc00f4df0300abdcf5db diff --git a/models/supercombo.dlc b/models/supercombo.dlc index 2952e70ec4..676a2ace37 100644 --- a/models/supercombo.dlc +++ b/models/supercombo.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:059efa4e4c5f1430271362147e4b2eda650dc7ca9bad6a63725cc616afdf7f77 -size 63447299 +oid sha256:dc46a24d4b4afa9730785264834e7f7c04c84b6a28a689acb503d6663818c256 +size 58797567 diff --git a/models/supercombo.onnx b/models/supercombo.onnx index 61a5e1e5bf..6b3ec952c1 100644 --- a/models/supercombo.onnx +++ b/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:f3c6f00e1e847a285fc0b834f610f4064d682d3e9f0878ba11fccb4e43692b4c -size 63313286 +oid sha256:77a31e5a3a70c39a3fcb07e939a668baf80b1eb778fe792fbe256de983fab5cd +size 58823636 diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h index b6282000ec..83aff9705c 100644 --- a/selfdrive/common/modeldata.h +++ b/selfdrive/common/modeldata.h @@ -3,14 +3,16 @@ const int TRAJECTORY_SIZE = 33; const float MIN_DRAW_DISTANCE = 10.0; const float MAX_DRAW_DISTANCE = 100.0; -const double T_IDXS[TRAJECTORY_SIZE] = {0. , 0.00976562, 0.0390625 , 0.08789062, 0.15625 , +const double T_IDXS[TRAJECTORY_SIZE] = { + 0. , 0.00976562, 0.0390625 , 0.08789062, 0.15625 , 0.24414062, 0.3515625 , 0.47851562, 0.625 , 0.79101562, 0.9765625 , 1.18164062, 1.40625 , 1.65039062, 1.9140625 , 2.19726562, 2.5 , 2.82226562, 3.1640625 , 3.52539062, 3.90625 , 4.30664062, 4.7265625 , 5.16601562, 5.625 , 6.10351562, 6.6015625 , 7.11914062, 7.65625 , 8.21289062, 8.7890625 , 9.38476562, 10.}; -const double X_IDXS[TRAJECTORY_SIZE] = { 0. , 0.1875, 0.75 , 1.6875, 3. , 4.6875, +const double X_IDXS[TRAJECTORY_SIZE] = { + 0. , 0.1875, 0.75 , 1.6875, 3. , 4.6875, 6.75 , 9.1875, 12. , 15.1875, 18.75 , 22.6875, 27. , 31.6875, 36.75 , 42.1875, 48. , 54.1875, 60.75 , 67.6875, 75. , 82.6875, 90.75 , 99.1875, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 616b0b8005..8a90d3c852 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -253,7 +253,7 @@ class Controls: self.events.add(EventName.deviceFalling) if log.PandaState.FaultType.relayMalfunction in self.sm['pandaState'].faults: self.events.add(EventName.relayMalfunction) - if self.sm['longitudinalPlan'].fcw: + if self.sm['longitudinalPlan'].fcw or (self.enabled and self.sm['modelV2'].meta.hardBrakePredicted): self.events.add(EventName.fcw) if TICI and self.enable_lte_onroad: diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index a54fd47885..def5b7e4cb 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -13,15 +13,17 @@ #include "selfdrive/common/timing.h" constexpr int DESIRE_PRED_SIZE = 32; -constexpr int OTHER_META_SIZE = 4; +constexpr int OTHER_META_SIZE = 32; +constexpr int NUM_META_INTERVALS = 5; +constexpr int META_STRIDE = 6; constexpr int MODEL_WIDTH = 512; constexpr int MODEL_HEIGHT = 256; constexpr int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2; constexpr int PLAN_MHP_N = 5; -constexpr int PLAN_MHP_COLUMNS = 30; -constexpr int PLAN_MHP_VALS = 30*33; +constexpr int PLAN_MHP_COLUMNS = 15; +constexpr int PLAN_MHP_VALS = 15*33; constexpr int PLAN_MHP_SELECTION = 1; constexpr int PLAN_MHP_GROUP_SIZE = (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION); @@ -35,7 +37,7 @@ constexpr int POSE_SIZE = 12; constexpr int PLAN_IDX = 0; constexpr int LL_IDX = PLAN_IDX + PLAN_MHP_N*PLAN_MHP_GROUP_SIZE; constexpr int LL_PROB_IDX = LL_IDX + 4*2*2*33; -constexpr int RE_IDX = LL_PROB_IDX + 4; +constexpr int RE_IDX = LL_PROB_IDX + 8; constexpr int LEAD_IDX = RE_IDX + 2*2*2*33; constexpr int LEAD_PROB_IDX = LEAD_IDX + LEAD_MHP_N*(LEAD_MHP_GROUP_SIZE); constexpr int DESIRE_STATE_IDX = LEAD_PROB_IDX + 3; @@ -48,6 +50,13 @@ constexpr int OUTPUT_SIZE = POSE_IDX + POSE_SIZE; constexpr int TEMPORAL_SIZE = 0; #endif +constexpr float FCW_THRESHOLD_5MS2_HIGH = 0.15; +constexpr float FCW_THRESHOLD_5MS2_LOW = 0.05; +constexpr float FCW_THRESHOLD_3MS2 = 0.7; + +float prev_brake_5ms2_probs[5] = {0,0,0,0,0}; +float prev_brake_3ms2_probs[3] = {0,0,0}; + // #define DUMP_YUV void model_init(ModelState* s, cl_device_id device_id, cl_context context) { @@ -151,6 +160,12 @@ static const float *get_lead_data(const float *lead, int t_offset) { } +void fill_sigmoid(const float *input, float *output, int len, int stride) { + for (int i=0; i threshold; + } + for (int i=0; i<3; i++) { + above_fcw_threshold = above_fcw_threshold && prev_brake_3ms2_probs[i] > FCW_THRESHOLD_3MS2; + } + + auto disengage = meta.initDisengagePredictions(); + disengage.setT({2,4,6,8,10}); + disengage.setGasDisengageProbs(gas_disengage_sigmoid); + disengage.setBrakeDisengageProbs(brake_disengage_sigmoid); + disengage.setSteerOverrideProbs(steer_override_sigmoid); + disengage.setBrake3MetersPerSecondSquaredProbs(brake_3ms2_sigmoid); + disengage.setBrake4MetersPerSecondSquaredProbs(brake_4ms2_sigmoid); + disengage.setBrake5MetersPerSecondSquaredProbs(brake_5ms2_sigmoid); + meta.setEngagedProb(sigmoid(meta_data[DESIRE_LEN])); - meta.setGasDisengageProb(sigmoid(meta_data[DESIRE_LEN + 1])); - meta.setBrakeDisengageProb(sigmoid(meta_data[DESIRE_LEN + 2])); - meta.setSteerOverrideProb(sigmoid(meta_data[DESIRE_LEN + 3])); meta.setDesirePrediction(desire_pred_softmax); + meta.setDesireState(desire_state_softmax); + meta.setHardBrakePredicted(above_fcw_threshold); } void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data, @@ -221,8 +272,25 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou // plan const float *best_plan = get_plan_data(net_outputs.plan); float plan_t_arr[TRAJECTORY_SIZE]; - for (int i=0; i