Skip to content

Commit

Permalink
torqued: rename lat_active (commaai#32942)
Browse files Browse the repository at this point in the history
* Update torqued.py

* lint

* not necessary
old-commit-hash: 35df0a4
  • Loading branch information
sshane authored Jul 9, 2024
1 parent ddde57d commit abd29fa
Showing 1 changed file with 3 additions and 3 deletions.
6 changes: 3 additions & 3 deletions selfdrive/locationd/torqued.py
Original file line number Diff line number Diff line change
Expand Up @@ -159,7 +159,7 @@ def update_params(self, params):
def handle_log(self, t, which, msg):
if which == "carControl":
self.raw_points["carControl_t"].append(t + self.lag)
self.raw_points["active"].append(msg.latActive)
self.raw_points["lat_active"].append(msg.latActive)
elif which == "carOutput":
self.raw_points["carOutput_t"].append(t + self.lag)
self.raw_points["steer_torque"].append(-msg.actuatorsOutput.steer)
Expand All @@ -171,12 +171,12 @@ def handle_log(self, t, which, msg):
if len(self.raw_points['steer_torque']) == self.hist_len:
yaw_rate = msg.angularVelocityCalibrated.value[2]
roll = msg.orientationNED.value[0]
active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t, DT_MDL), self.raw_points['carControl_t'], self.raw_points['active']).astype(bool)
lat_active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t, DT_MDL), self.raw_points['carControl_t'], self.raw_points['lat_active']).astype(bool)
steer_override = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t, DT_MDL), self.raw_points['carState_t'], self.raw_points['steer_override']).astype(bool)
vego = np.interp(t, self.raw_points['carState_t'], self.raw_points['vego'])
steer = np.interp(t, self.raw_points['carOutput_t'], self.raw_points['steer_torque'])
lateral_acc = (vego * yaw_rate) - (np.sin(roll) * ACCELERATION_DUE_TO_GRAVITY)
if all(active) and (not any(steer_override)) and (vego > MIN_VEL) and (abs(steer) > STEER_MIN_THRESHOLD) and (abs(lateral_acc) <= LAT_ACC_THRESHOLD):
if all(lat_active) and not any(steer_override) and (vego > MIN_VEL) and (abs(steer) > STEER_MIN_THRESHOLD) and (abs(lateral_acc) <= LAT_ACC_THRESHOLD):
self.filtered_points.add_point(float(steer), float(lateral_acc))

def get_msg(self, valid=True, with_points=False):
Expand Down

0 comments on commit abd29fa

Please sign in to comment.