diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index ad84ecf24..c5c03eba1 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -152,10 +152,11 @@ class LongitudinalPlanner: output_a_target_e2e = sm['modelV2'].action.desiredAcceleration output_should_stop_e2e = sm['modelV2'].action.shouldStop - if (output_a_target_e2e < output_a_target_mpc) and sm['selfdriveState'].experimentalMode: - output_a_target = output_a_target_e2e - self.output_should_stop = output_should_stop_e2e - self.mpc.source = SOURCES[3] + if sm['selfdriveState'].experimentalMode: + output_a_target = min(output_a_target_e2e, output_a_target_mpc) + self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc + if output_a_target < output_a_target_mpc: + self.mpc.source = SOURCES[3] else: output_a_target = output_a_target_mpc self.output_should_stop = output_should_stop_mpc