sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

torqued: rename lat_active (#32942)

Browse Source 
* Update torqued.py

* lint

* not necessary
pull/32948/head
Shane Smiskol 10 months ago committed by GitHub
parent 481e5b28bd
commit 35df0a4fda
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 3 additions and 3 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      selfdrive/locationd/torqued.py

6
selfdrive/locationd/torqued.py
Unescape View File

@ -159,7 +159,7 @@ class TorqueEstimator(ParameterEstimator):
def handle_log(self, t, which, msg): def handle_log(self, t, which, msg):
if which == "carControl": if which == "carControl":
self.raw_points["carControl_t"].append(t + self.lag) 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": elif which == "carOutput":
self.raw_points["carOutput_t"].append(t + self.lag) self.raw_points["carOutput_t"].append(t + self.lag)
self.raw_points["steer_torque"].append(-msg.actuatorsOutput.steer) self.raw_points["steer_torque"].append(-msg.actuatorsOutput.steer)
@ -171,12 +171,12 @@ class TorqueEstimator(ParameterEstimator):
if len(self.raw_points['steer_torque']) == self.hist_len: if len(self.raw_points['steer_torque']) == self.hist_len:
yaw_rate = msg.angularVelocityCalibrated.value[2] yaw_rate = msg.angularVelocityCalibrated.value[2]
roll = msg.orientationNED.value[0] 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) 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']) 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']) 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) 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)) self.filtered_points.add_point(float(steer), float(lateral_acc))
def get_msg(self, valid=True, with_points=False): def get_msg(self, valid=True, with_points=False):

Write Preview
Loading…
Cancel
Save