use calibrated roll

pull/35700/head
Shane Smiskol 3 weeks ago
parent 4e83fbec83
commit 5d8c605717
  1. 4
      selfdrive/selfdrived/selfdrived.py

@ -55,7 +55,7 @@ def check_excessive_actuation(sm: messaging.SubMaster, CS: car.CarState, calibra
# lateral # lateral
yaw_rate = calibrated_pose.angular_velocity.yaw yaw_rate = calibrated_pose.angular_velocity.yaw
roll = device_pose.orientation.roll # TODO: calibrated_pose? roll = sm['liveParameters'].roll
roll_compensated_lateral_accel = (CS.vEgo * yaw_rate) - (math.sin(roll) * ACCELERATION_DUE_TO_GRAVITY) roll_compensated_lateral_accel = (CS.vEgo * yaw_rate) - (math.sin(roll) * ACCELERATION_DUE_TO_GRAVITY)
# livePose acceleration can be noisy due to bad mounting or aliased livePose measurements # livePose acceleration can be noisy due to bad mounting or aliased livePose measurements
@ -83,6 +83,7 @@ class SelfdriveD:
self.CP = CP self.CP = CP
self.car_events = CarSpecificEvents(self.CP) self.car_events = CarSpecificEvents(self.CP)
self.calibrator = PoseCalibrator()
# Setup sockets # Setup sockets
self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents']) self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'])
@ -144,7 +145,6 @@ class SelfdriveD:
self.excessive_actuation_counter = 0 self.excessive_actuation_counter = 0
self.state_machine = StateMachine() self.state_machine = StateMachine()
self.rk = Ratekeeper(100, print_delay_threshold=None) self.rk = Ratekeeper(100, print_delay_threshold=None)
self.calibrator = PoseCalibrator()
# some comma three with NVMe experience NVMe dropouts mid-drive that # some comma three with NVMe experience NVMe dropouts mid-drive that
# cause loggerd to crash on write, so ignore it only on that platform # cause loggerd to crash on write, so ignore it only on that platform

Loading…
Cancel
Save