|
|
|
@ -1,8 +1,8 @@ |
|
|
|
|
#!/usr/bin/env python3 |
|
|
|
|
import os |
|
|
|
|
import math |
|
|
|
|
import time |
|
|
|
|
import threading |
|
|
|
|
import numpy as np |
|
|
|
|
|
|
|
|
|
import cereal.messaging as messaging |
|
|
|
|
|
|
|
|
@ -56,7 +56,7 @@ def check_excessive_actuation(sm: messaging.SubMaster, CS: car.CarState, calibra |
|
|
|
|
# lateral |
|
|
|
|
yaw_rate = calibrated_pose.angular_velocity.yaw |
|
|
|
|
roll = device_pose.orientation.roll # TODO: calibrated_pose? |
|
|
|
|
roll_compensated_lateral_accel = float((CS.vEgo * yaw_rate) - (np.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 |
|
|
|
|
accel_valid = abs(CS.aEgo - accel_calibrated) < 2 |
|
|
|
|