|
|
@ -90,7 +90,7 @@ class LongitudinalPlanner: |
|
|
|
return x, v, a, j, throttle_prob |
|
|
|
return x, v, a, j, throttle_prob |
|
|
|
|
|
|
|
|
|
|
|
def update(self, sm): |
|
|
|
def update(self, sm): |
|
|
|
self.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' |
|
|
|
mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' |
|
|
|
|
|
|
|
|
|
|
|
if len(sm['carControl'].orientationNED) == 3: |
|
|
|
if len(sm['carControl'].orientationNED) == 3: |
|
|
|
accel_coast = get_coast_accel(sm['carControl'].orientationNED[1]) |
|
|
|
accel_coast = get_coast_accel(sm['carControl'].orientationNED[1]) |
|
|
@ -113,7 +113,7 @@ class LongitudinalPlanner: |
|
|
|
# No change cost when user is controlling the speed, or when standstill |
|
|
|
# No change cost when user is controlling the speed, or when standstill |
|
|
|
prev_accel_constraint = not (reset_state or sm['carState'].standstill) |
|
|
|
prev_accel_constraint = not (reset_state or sm['carState'].standstill) |
|
|
|
|
|
|
|
|
|
|
|
if self.mode == 'acc': |
|
|
|
if mode == 'acc': |
|
|
|
accel_clip = [ACCEL_MIN, get_max_accel(v_ego)] |
|
|
|
accel_clip = [ACCEL_MIN, get_max_accel(v_ego)] |
|
|
|
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg |
|
|
|
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg |
|
|
|
accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP) |
|
|
|
accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP) |
|
|
@ -163,7 +163,7 @@ class LongitudinalPlanner: |
|
|
|
output_a_target_e2e = sm['modelV2'].action.desiredAcceleration |
|
|
|
output_a_target_e2e = sm['modelV2'].action.desiredAcceleration |
|
|
|
output_should_stop_e2e = sm['modelV2'].action.shouldStop |
|
|
|
output_should_stop_e2e = sm['modelV2'].action.shouldStop |
|
|
|
|
|
|
|
|
|
|
|
if self.mode == 'acc': |
|
|
|
if mode == 'acc': |
|
|
|
output_a_target = output_a_target_mpc |
|
|
|
output_a_target = output_a_target_mpc |
|
|
|
self.output_should_stop = output_should_stop_mpc |
|
|
|
self.output_should_stop = output_should_stop_mpc |
|
|
|
else: |
|
|
|
else: |
|
|
|