def test_send(self):
socks = random_socks()
pm = messaging.PubMaster(socks)
sub_socks = {
s: messaging.sub_sock(s, conflate=True, timeout=1000)
for s in socks
}
zmq_sleep()
# PubMaster accepts either a capnp msg builder or bytes
for capnp in [True, False]:
for i in range(100):
sock = socks[i % len(socks)]
if capnp:
try:
msg = messaging.new_message(sock)
except Exception:
msg = messaging.new_message(sock, random.randrange(50))
else:
msg = random_bytes()
pm.send(sock, msg)
recvd = sub_socks[sock].receive()
if capnp:
msg.clear_write_flag()
msg = msg.to_bytes()
self.assertEqual(msg, recvd, i)
def _send_cur_state(self):
if self.mpc_id == 1 and self.pm is not None:
dat = messaging_arne.new_message()
dat.init('dynamicFollowData')
dat.dynamicFollowData.mpcTR = 1.8 # self.TR # FIX THIS! sometimes nonetype
dat.dynamicFollowData.profilePred = self.model_profile
self.pm.send('dynamicFollowData', dat)
def read_thermal():
dat = messaging.new_message()
dat.init('thermalonline')
dat.thermalonline.cpu0 = read_tz(5)
dat.thermalonline.cpu1 = read_tz(7)
dat.thermalonline.cpu2 = read_tz(10)
dat.thermalonline.cpu3 = read_tz(12)
dat.thermalonline.mem = read_tz(2)
dat.thermalonline.gpu = read_tz(16)
dat.thermalonline.bat = read_tz(29)
dat.thermalonline.pa0 = read_tz(25)
return dat
def test_update(self):
sock = "carState"
pub_sock = messaging.pub_sock(sock)
sm = messaging.SubMaster([
sock,
])
zmq_sleep()
for i in range(10):
msg = messaging.new_message(sock)
pub_sock.send(msg.to_bytes())
sm.update(1000)
self.assertEqual(sm.frame, i)
self.assertTrue(all(sm.updated.values()))
def test_conflate(self):
sock = "carState"
pub_sock = messaging.pub_sock(sock)
sm = messaging.SubMaster([
sock,
])
n = 10
for i in range(n + 1):
msg = messaging.new_message(sock)
msg.carState.vEgo = i
pub_sock.send(msg.to_bytes())
time.sleep(0.01)
sm.update(1000)
self.assertEqual(sm[sock].vEgo, n)
def send_prediction(self, pred, confidence):
traffic_send = messaging_arne.new_message('trafficModelEvent')
traffic_send.trafficModelEvent.status = pred
traffic_send.trafficModelEvent.confidence = float(confidence)
self.pm.send('trafficModelEvent', traffic_send)
def update(self, cp, cp_cam, frame):
ret = car.CarState.new_message()
ret.doorOpen = any([cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'],
cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']])
ret.seatbeltUnlatched = cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED'] != 0
ret.brakePressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] != 0
ret.brakeLights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or ret.brakePressed)
if self.CP.enableGasInterceptor:
ret.gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
ret.gasPressed = ret.gas > 15
else:
ret.gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
ret.gasPressed = cp.vl["PCM_CRUISE"]['GAS_RELEASED'] == 0
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.001
# Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero
if abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']) > 1e-3:
self.accurate_steer_angle_seen = True
if self.accurate_steer_angle_seen:
ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
if self.needs_angle_offset:
angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3:
self.needs_angle_offset = False
self.angle_offset = ret.steeringAngle - angle_wheel
else:
ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
ret.steeringRate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
try:
self.econ_on = cp.vl["GEAR_PACKET"]['ECON_ON']
except:
self.econ_on = 0
if self.CP.carFingerprint in [CAR.COROLLAH_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_UXH_TSS2, CAR.CHRH]:
self.econ_on = cp.vl["GEAR_PACKET2"]['ECON_ON']
try:
self.sport_on = cp.vl["GEAR_PACKET"]['SPORT_ON']
except:
self.sport_on = 0
if self.CP.carFingerprint in [CAR.COROLLAH_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_UXH_TSS2, CAR.CHRH]:
self.sport_on = cp.vl["GEAR_PACKET2"]['SPORT_ON']
if self.sport_on == 1:
self.gasbuttonstatus = 1
if self.econ_on == 1:
self.gasbuttonstatus = 2
if self.sport_on == 0 and self.econ_on == 0:
self.gasbuttonstatus = 0
msg = messaging_arne.new_message('arne182Status')
if frame > 999 and not (self.CP.carFingerprint in TSS2_CAR):
if cp.vl["DEBUG"]['BLINDSPOTSIDE']==65: #Left
if cp.vl["DEBUG"]['BLINDSPOTD1'] != self.leftblindspotD1:
self.leftblindspotD1 = cp.vl["DEBUG"]['BLINDSPOTD1']
self.leftblindspotcounter = 21
if cp.vl["DEBUG"]['BLINDSPOTD2'] != self.leftblindspotD2:
self.leftblindspotD2 = cp.vl["DEBUG"]['BLINDSPOTD2']
self.leftblindspotcounter = 21
if (self.leftblindspotD1 > 10) or (self.leftblindspotD2 > 10):
self.leftblindspot = bool(1)
print("Left Blindspot Detected")
elif cp.vl["DEBUG"]['BLINDSPOTSIDE']==66: #Right
if cp.vl["DEBUG"]['BLINDSPOTD1'] != self.rightblindspotD1:
self.rightblindspotD1 = cp.vl["DEBUG"]['BLINDSPOTD1']
self.rightblindspotcounter = 21
if cp.vl["DEBUG"]['BLINDSPOTD2'] != self.rightblindspotD2:
self.rightblindspotD2 = cp.vl["DEBUG"]['BLINDSPOTD2']
self.rightblindspotcounter = 21
if (self.rightblindspotD1 > 10) or (self.rightblindspotD2 > 10):
self.rightblindspot = bool(1)
print("Right Blindspot Detected")
self.rightblindspotcounter = self.rightblindspotcounter -1 if self.rightblindspotcounter > 0 else 0
self.leftblindspotcounter = self.leftblindspotcounter -1 if self.leftblindspotcounter > 0 else 0
if self.leftblindspotcounter == 0:
self.leftblindspot = False
self.leftblindspotD1 = 0
self.leftblindspotD2 = 0
if self.rightblindspotcounter == 0:
self.rightblindspot = False
self.rightblindspotD1 = 0
self.rightblindspotD2 = 0
elif frame > 999 and self.CP.carFingerprint in TSS2_CAR:
self.leftblindspot = cp.vl["BSM"]['L_ADJACENT'] == 1
self.leftblindspotD1 = 10.1
self.leftblindspotD2 = 10.1
self.rightblindspot = cp.vl["BSM"]['R_ADJACENT'] == 1
self.rightblindspotD1 = 10.1
self.rightblindspotD2 = 10.1
msg.arne182Status.leftBlindspot = self.leftblindspot
ret.leftBlindspot = self.leftblindspot
msg.arne182Status.rightBlindspot = self.rightblindspot
ret.rightBlindspot = self.rightblindspot
msg.arne182Status.rightBlindspotD1 = self.rightblindspotD1
msg.arne182Status.rightBlindspotD2 = self.rightblindspotD2
msg.arne182Status.leftBlindspotD1 = self.leftblindspotD1
msg.arne182Status.leftBlindspotD2 = self.leftblindspotD2
msg.arne182Status.gasbuttonstatus = self.gasbuttonstatus
if not travis:
self.arne_sm.update(0)
self.sm.update(0)
self.smartspeed = self.sm['liveMapData'].speedLimit
self.arne_pm.send('arne182Status', msg)
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
ret.rightBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']
ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS']
# we could use the override bit from dbc, but it's triggered at too high torque values
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
ret.steerWarning = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5]
if self.CP.carFingerprint == CAR.LEXUS_IS:
self.main_on = cp.vl["DSU_CRUISE"]['MAIN_ON'] != 0
ret.cruiseState.speed = cp.vl["DSU_CRUISE"]['SET_SPEED']
self.low_speed_lockout = False
else:
self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON'] != 0
ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
if self.CP.carFingerprint == CAR.COROLLAH_TSS2:
self.low_speed_lockout = False
else:
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
ret.cruiseState.available = self.main_on
v_cruise_pcm_max = ret.cruiseState.speed
if self.CP.carFingerprint in TSS2_CAR:
minimum_set_speed = 27.0
elif self.CP.carFingerprint == CAR.RAV4:
minimum_set_speed = 44.0
else:
minimum_set_speed = 41.0
maximum_set_speed = 169.0
if self.CP.carFingerprint == CAR.LEXUS_RXH:
maximum_set_speed = 177.0
speed_range = maximum_set_speed-minimum_set_speed
if bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE']) and not self.pcm_acc_active and self.v_cruise_pcmlast != ret.cruiseState.speed:
if ret.vEgo < 12.5:
self.setspeedoffset = max(min(int(minimum_set_speed-ret.vEgo*3.6),(minimum_set_speed-7.0)),0.0)
self.v_cruise_pcmlast = ret.cruiseState.speed
if ret.cruiseState.speed < self.v_cruise_pcmlast:
if self.setspeedcounter > 0 and ret.cruiseState.speed > minimum_set_speed:
self.setspeedoffset = self.setspeedoffset + 4
else:
if math.floor((int((-ret.cruiseState.speed)*(minimum_set_speed-7.0)/speed_range + maximum_set_speed*(minimum_set_speed-7.0)/speed_range)-self.setspeedoffset)/(ret.cruiseState.speed-(minimum_set_speed-1.0))) > 0:
self.setspeedoffset = self.setspeedoffset + math.floor((int((-ret.cruiseState.speed)*(minimum_set_speed-7.0)/speed_range + maximum_set_speed*(minimum_set_speed-7.0)/speed_range)-self.setspeedoffset)/(ret.cruiseState.speed-(minimum_set_speed-1.0)))
self.setspeedcounter = 50
if self.v_cruise_pcmlast < ret.cruiseState.speed:
if self.setspeedcounter > 0 and (self.setspeedoffset - 4) > 0:
self.setspeedoffset = self.setspeedoffset - 4
else:
self.setspeedoffset = self.setspeedoffset + math.floor((int((-ret.cruiseState.speed)*(minimum_set_speed-7.0)/speed_range + maximum_set_speed*(minimum_set_speed-7.0)/speed_range)-self.setspeedoffset)/(maximum_set_speed+1.0-ret.cruiseState.speed))
self.setspeedcounter = 50
if self.setspeedcounter > 0:
self.setspeedcounter = self.setspeedcounter - 1
self.v_cruise_pcmlast = ret.cruiseState.speed
if int(ret.cruiseState.speed) - self.setspeedoffset < 7:
self.setspeedoffset = int(ret.cruiseState.speed) - 7
if int(ret.cruiseState.speed) - self.setspeedoffset > maximum_set_speed:
self.setspeedoffset = int(ret.cruiseState.speed) - maximum_set_speed
ret.cruiseState.speed = min(max(7, int(ret.cruiseState.speed) - self.setspeedoffset),v_cruise_pcm_max)
if not travis and self.arne_sm.updated['latControl'] and ret.vEgo > 11:
angle_later = self.arne_sm['latControl'].anglelater
else:
angle_later = 0
if not self.left_blinker_on and not self.right_blinker_on:
self.Angles[self.Angle_counter] = abs(ret.steeringAngle)
self.Angles_later[self.Angle_counter] = abs(angle_later)
else:
self.Angles[self.Angle_counter] = abs(ret.steeringAngle) * 0.8
if ret.vEgo > 11:
self.Angles_later[self.Angle_counter] = abs(angle_later) * 0.8
else:
self.Angles_later[self.Angle_counter] = 0.0
if self.gasbuttonstatus ==1:
factor = 1.6
elif self.gasbuttonstatus == 2:
factor = 1.0
else:
factor = 1.3
ret.cruiseState.speed = int(min(ret.cruiseState.speed, factor * interp(np.max(self.Angles), self.Angle, self.Angle_Speed)))
ret.cruiseState.speed = int(min(ret.cruiseState.speed, factor * interp(np.max(self.Angles_later), self.Angle, self.Angle_Speed)))
self.Angle_counter = (self.Angle_counter + 1 ) % 250
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor:
# ignore standstill in hybrid vehicles, since pcm allows to restart without
# receiving any special command. Also if interceptor is detected
ret.cruiseState.standstill = False
else:
ret.cruiseState.standstill = self.pcm_acc_status == 7
self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
ret.cruiseState.enabled = self.pcm_acc_active
if self.CP.carFingerprint in [CAR.PRIUS, CAR.PRIUS_2019]:
ret.genericToggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
else:
ret.genericToggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
ret.stockAeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
ret.espDisabled = cp.vl["ESP_CONTROL"]['TC_DISABLED'] != 0
# 2 is standby, 10 is active. TODO: check that everything else is really a faulty state
self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE']
self.steer_warning = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5]
ret.cruiseState.speed = ret.cruiseState.speed * CV.KPH_TO_MS
#self.barriers = cp_cam.vl["LKAS_HUD"]['BARRIERS']
#self.rightline = cp_cam.vl["LKAS_HUD"]['RIGHT_LINE']
#self.leftline = cp_cam.vl["LKAS_HUD"]['LEFT_LINE']
self.tsgn1 = cp_cam.vl["RSA1"]['TSGN1']
if self.spdval1 != cp_cam.vl["RSA1"]['SPDVAL1']:
self.rsa_ignored_speed = 0
self.spdval1 = cp_cam.vl["RSA1"]['SPDVAL1']
self.splsgn1 = cp_cam.vl["RSA1"]['SPLSGN1']
self.tsgn2 = cp_cam.vl["RSA1"]['TSGN2']
#self.spdval2 = cp_cam.vl["RSA1"]['SPDVAL2']
self.splsgn2 = cp_cam.vl["RSA1"]['SPLSGN2']
self.tsgn3 = cp_cam.vl["RSA2"]['TSGN3']
self.splsgn3 = cp_cam.vl["RSA2"]['SPLSGN3']
self.tsgn4 = cp_cam.vl["RSA2"]['TSGN4']
self.splsgn4 = cp_cam.vl["RSA2"]['SPLSGN4']
self.noovertake = self.tsgn1 == 65 or self.tsgn2 == 65 or self.tsgn3 == 65 or self.tsgn4 == 65 or self.tsgn1 == 66 or self.tsgn2 == 66 or self.tsgn3 == 66 or self.tsgn4 == 66
if (self.spdval1 > 0) and not (self.spdval1 == 35 and self.tsgn1 == 1) and self.rsa_ignored_speed != self.spdval1:
dat = messaging_arne.new_message('liveTrafficData')
if self.spdval1 > 0:
dat.liveTrafficData.speedLimitValid = True
if self.tsgn1 == 36:
dat.liveTrafficData.speedLimit = self.spdval1 * 1.60934
elif self.tsgn1 == 1:
dat.liveTrafficData.speedLimit = self.spdval1
else:
dat.liveTrafficData.speedLimit = 0
else:
dat.liveTrafficData.speedLimitValid = False
#if self.spdval2 > 0:
# dat.liveTrafficData.speedAdvisoryValid = True
# dat.liveTrafficData.speedAdvisory = self.spdval2
#else:
dat.liveTrafficData.speedAdvisoryValid = False
if limit_rsa and rsa_max_speed < ret.vEgo:
dat.liveTrafficData.speedLimitValid = False
if not travis:
self.arne_pm.send('liveTrafficData', dat)
if ret.gasPressed and not self.gas_pressed:
self.engaged_when_gas_was_pressed = self.pcm_acc_active
if ((ret.gasPressed) or (self.gas_pressed and not ret.gasPressed)) and self.engaged_when_gas_was_pressed and ret.vEgo > self.smartspeed:
self.rsa_ignored_speed = self.spdval1
dat = messaging_arne.new_message('liveTrafficData')
dat.liveTrafficData.speedLimitValid = True
dat.liveTrafficData.speedLimit = ret.vEgo * 3.6
if not travis:
self.arne_pm.send('liveTrafficData', dat)
self.gas_pressed = ret.gasPressed
return ret
def update(self, cp, cp_cam):
# update prevs, update must run once per loop
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
self.door_all_closed = not any([
cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'],
cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'],
cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'],
cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']
])
self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED']
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
if self.CP.enableGasInterceptor:
self.pedal_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] +
cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
else:
self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
self.car_gas = self.pedal_gas
self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED']
# calc best v_ego estimate, by averaging two opposite corners
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
v_wheel = mean([
self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr
])
# Kalman filter
if abs(
v_wheel - self.v_ego
) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = [[v_wheel], [0.0]]
self.v_ego_raw = v_wheel
v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
self.standstill = not v_wheel > 0.001
if self.CP.carFingerprint in TSS2_CAR:
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
elif self.CP.carFingerprint in NO_DSU_CAR:
# cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] is zeroed to where the steering angle is at start.
# need to apply an offset as soon as the steering angle measurements are both received
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"][
'STEER_ANGLE'] - self.angle_offset
angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl[
"STEER_ANGLE_SENSOR"]['STEER_FRACTION']
if abs(angle_wheel) > 1e-3 and abs(
self.angle_steers) > 1e-3 and not self.init_angle_offset:
self.init_angle_offset = True
self.angle_offset = self.angle_steers - angle_wheel
else:
self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"][
'STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
self.gear_shifter = parse_gear_shifter(can_gear, self.shifter_values)
if self.CP.carFingerprint == CAR.LEXUS_IS:
self.main_on = cp.vl["DSU_CRUISE"]['MAIN_ON']
else:
self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON']
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
# 2 is standby, 10 is active. TODO: check that everything else is really a faulty state
self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE']
self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5]
self.ipas_active = cp.vl['EPS_STATUS']['IPAS_STATE'] == 3
self.brake_error = 0
self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"][
'STEER_TORQUE_DRIVER']
self.steer_torque_motor = cp.vl["STEER_TORQUE_SENSOR"][
'STEER_TORQUE_EPS']
# we could use the override bit from dbc, but it's triggered at too high torque values
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
self.user_brake = 0
if self.CP.carFingerprint == CAR.LEXUS_IS:
self.v_cruise_pcm = cp.vl["DSU_CRUISE"]['SET_SPEED']
self.low_speed_lockout = False
else:
self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"][
'LOW_SPEED_LOCKOUT'] == 2
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC']
or self.brake_pressed)
if self.CP.carFingerprint == CAR.PRIUS:
self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
else:
self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
self.stock_aeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"]
and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
self.barriers = cp_cam.vl["LKAS_HUD"]['BARRIERS']
self.rightline = cp_cam.vl["LKAS_HUD"]['RIGHT_LINE']
self.leftline = cp_cam.vl["LKAS_HUD"]['LEFT_LINE']
self.tsgn1 = cp_cam.vl["RSA1"]['TSGN1']
self.spdval1 = cp_cam.vl["RSA1"]['SPDVAL1']
self.splsgn1 = cp_cam.vl["RSA1"]['SPLSGN1']
self.tsgn2 = cp_cam.vl["RSA1"]['TSGN2']
self.spdval2 = cp_cam.vl["RSA1"]['SPDVAL2']
self.splsgn2 = cp_cam.vl["RSA1"]['SPLSGN2']
self.tsgn3 = cp_cam.vl["RSA2"]['TSGN3']
self.splsgn3 = cp_cam.vl["RSA2"]['SPLSGN3']
self.tsgn4 = cp_cam.vl["RSA2"]['TSGN4']
self.splsgn4 = cp_cam.vl["RSA2"]['SPLSGN4']
self.noovertake = self.tsgn1 == 65 or self.tsgn2 == 65 or self.tsgn3 == 65 or self.tsgn4 == 65 or self.tsgn1 == 66 or self.tsgn2 == 66 or self.tsgn3 == 66 or self.tsgn4 == 66
if self.spdval1 > 0 or self.spdval2 > 0:
dat = messaging_arne.new_message()
dat.init('liveTrafficData')
if self.spdval1 > 0:
dat.liveTrafficData.speedLimitValid = True
if self.tsgn1 == 36:
dat.liveTrafficData.speedLimit = self.spdval1 * 1.60934
elif self.tsgn1 == 1:
dat.liveTrafficData.speedLimit = self.spdval1
else:
dat.liveTrafficData.speedLimit = 0
else:
dat.liveTrafficData.speedLimitValid = False
if self.spdval2 > 0:
dat.liveTrafficData.speedAdvisoryValid = True
dat.liveTrafficData.speedAdvisory = self.spdval2
else:
dat.liveTrafficData.speedAdvisoryValid = False
if not travis:
self.arne_pm.send('liveTrafficData', dat)
def update(self, sm, pm, CP, VM):
if not travis:
self.arne_sm.update(0)
gas_button_status = self.arne_sm['arne182Status'].gasbuttonstatus
if gas_button_status == 1:
self.blindspotwait = 10
elif gas_button_status == 2:
self.blindspotwait = 30
else:
self.blindspotwait = 20
if self.arne_sm['arne182Status'].rightBlindspot:
self.blindspotTrueCounterright = 0
else:
self.blindspotTrueCounterright = self.blindspotTrueCounterright + 1
if self.arne_sm['arne182Status'].leftBlindspot:
self.blindspotTrueCounterleft = 0
else:
self.blindspotTrueCounterleft = self.blindspotTrueCounterleft + 1
else:
self.blindspotwait = 10
self.blindspotTrueCounterleft = 0
self.blindspotTrueCounterright = 0
v_ego = sm['carState'].vEgo
angle_steers = sm['carState'].steeringAngle
active = sm['controlsState'].active
angle_offset = sm['liveParameters'].angleOffset
# Run MPC
self.angle_steers_des_prev = self.angle_steers_des_mpc
#VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
#VM.update_params(sm['liveParameters'].stiffnessFactor, CP.steerRatio)
VM.update_params(sm['liveParameters'].stiffnessFactor, self.steerRatio)
curvature_factor = VM.curvature_factor(v_ego)
# Get steerRatio and steerRateCost from kegman.json every x seconds
self.mpc_frame += 1
if self.mpc_frame % 500 == 0:
# live tuning through /data/openpilot/tune.py overrides interface.py settings
self.sR_Boost_new = interp(v_ego, self.sR_BoostBP, self.sR_Boost)
self.steerRatio = self.op_params.get('steer_ratio', default=12.0)
self.sR = [self.steerRatio, self.steerRatio + self.sR_Boost_new]
self.sR_time = 100
self.mpc_frame = 0
if v_ego > 5.0:
# boost steerRatio by boost amount if desired steer angle is high
self.steerRatio_new = interp(abs(angle_steers), self.sRBP, self.sR)
self.sR_delay_counter += 1
if self.sR_delay_counter % self.sR_time != 0:
if self.steerRatio_new > self.steerRatio:
self.steerRatio = self.steerRatio_new
else:
self.steerRatio = self.steerRatio_new
self.sR_delay_counter = 0
else:
self.steerRatio = self.sR[0]
print("steerRatio = ", self.steerRatio)
self.LP.parse_model(sm['model'])
# Lane change logic
one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
below_lane_change_speed = v_ego < self.alca_min_speed * CV.MPH_TO_MS
if sm['carState'].leftBlinker:
self.lane_change_direction = LaneChangeDirection.left
elif sm['carState'].rightBlinker:
self.lane_change_direction = LaneChangeDirection.right
if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (
not one_blinker
) or (not self.lane_change_enabled) or (
sm['carState'].steeringPressed and
((sm['carState'].steeringTorque > 0
and self.lane_change_direction == LaneChangeDirection.right) or
(sm['carState'].steeringTorque < 0
and self.lane_change_direction == LaneChangeDirection.left))):
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
else:
if sm['carState'].leftBlinker:
lane_change_direction = LaneChangeDirection.left
elif sm['carState'].rightBlinker:
lane_change_direction = LaneChangeDirection.right
#if self.alca_nudge_required:
torque_applied = (sm['carState'].steeringPressed and \
((sm['carState'].steeringTorque > 0 and lane_change_direction == LaneChangeDirection.left and not sm['carState'].leftBlindspot) or \
(sm['carState'].steeringTorque < 0 and lane_change_direction == LaneChangeDirection.right and not sm['carState'].rightBlindspot))) or \
(not self.alca_nudge_required and self.blindspotTrueCounterleft > self.blindspotwait and lane_change_direction == LaneChangeDirection.left) or \
(not self.alca_nudge_required and self.blindspotTrueCounterright > self.blindspotwait and lane_change_direction == LaneChangeDirection.right)
#else:
# torque_applied = True
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
# State transitions
# off
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
self.blindspotTrueCounterleft = 0
self.blindspotTrueCounterright = 0
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_ll_prob = 1.0
# pre
elif self.lane_change_state == LaneChangeState.preLaneChange:
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
self.blindspotTrueCounterleft = 0
self.blindspotTrueCounterright = 0
elif torque_applied:
self.lane_change_state = LaneChangeState.laneChangeStarting
# starting
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
# fade out over .2s
self.lane_change_ll_prob = max(
self.lane_change_ll_prob - 2 * DT_MDL, 0.0)
# 98% certainty
if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
self.lane_change_state = LaneChangeState.laneChangeFinishing
if (self.arne_sm['arne182Status'].rightBlindspot
and lane_change_direction == LaneChangeDirection.right
) or (self.arne_sm['arne182Status'].leftBlindspot and
lane_change_direction == LaneChangeDirection.left):
self.lane_change_state = LaneChangeState.preLaneChange
self.blindspotTrueCounterleft = 0
self.blindspotTrueCounterright = 0
# finishing
elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
# fade in laneline over 1s
self.lane_change_ll_prob = min(
self.lane_change_ll_prob + 0.5 * DT_MDL, 1.0)
if one_blinker and self.lane_change_ll_prob > 0.99:
self.lane_change_state = LaneChangeState.preLaneChange
self.blindspotTrueCounterleft = 0
self.blindspotTrueCounterright = 0
elif self.lane_change_ll_prob > 0.99:
self.lane_change_state = LaneChangeState.off
self.blindspotTrueCounterright = 0
self.blindspotTrueCounterleft = 0
if self.lane_change_state in [
LaneChangeState.off, LaneChangeState.preLaneChange
]:
self.lane_change_timer = 0.0
else:
self.lane_change_timer += DT_MDL
self.prev_one_blinker = one_blinker
desire = DESIRES[self.lane_change_direction][self.lane_change_state]
# Turn off lanes during lane change
if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft:
self.LP.l_prob *= self.lane_change_ll_prob
self.LP.r_prob *= self.lane_change_ll_prob
self.LP.update_d_poly(v_ego)
# account for actuation delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego,
angle_steers - angle_offset,
curvature_factor,
self.steerRatio,
CP.steerActuatorDelay)
#if abs(angle_steers - angle_offset) > 4 and CP.lateralTuning.which() == 'pid': #check if this causes laggy steering
# self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
list(self.LP.l_poly), list(self.LP.r_poly),
list(self.LP.d_poly), self.LP.l_prob,
self.LP.r_prob, curvature_factor, v_ego_mpc,
self.LP.lane_width)
# reset to current steer angle if not active or overriding
if active:
delta_desired = self.mpc_solution[0].delta[1]
rate_desired = math.degrees(self.mpc_solution[0].rate[0] *
self.steerRatio)
else:
delta_desired = math.radians(angle_steers -
angle_offset) / self.steerRatio
rate_desired = 0.0
self.cur_state[0].delta = delta_desired
self.angle_steers_des_mpc = float(
math.degrees(delta_desired * self.steerRatio) + angle_offset)
# Check for infeasable MPC solution
mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta)
t = sec_since_boot()
if mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE,
MPC_COST_LAT.HEADING, CP.steerRateCost)
self.cur_state[0].delta = math.radians(
angle_steers - angle_offset) / self.steerRatio
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
if self.mpc_solution[
0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge
self.solution_invalid_cnt += 1
else:
self.solution_invalid_cnt = 0
plan_solution_valid = self.solution_invalid_cnt < 2
plan_send = messaging.new_message('pathPlan')
plan_send.valid = sm.all_alive_and_valid(service_list=[
'carState', 'controlsState', 'liveParameters', 'model'
])
plan_send.pathPlan.laneWidth = float(self.LP.lane_width)
plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly]
plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly]
plan_send.pathPlan.lProb = float(self.LP.l_prob)
plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly]
plan_send.pathPlan.rProb = float(self.LP.r_prob)
plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
plan_send.pathPlan.rateSteers = float(rate_desired)
plan_send.pathPlan.angleOffset = float(
sm['liveParameters'].angleOffsetAverage)
plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid)
plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid)
plan_send.pathPlan.posenetValid = bool(
sm['liveParameters'].posenetValid) or self.posenetValid
self.posenetValid = bool(sm['liveParameters'].posenetValid)
plan_send.pathPlan.desire = desire
plan_send.pathPlan.laneChangeState = self.lane_change_state
plan_send.pathPlan.laneChangeDirection = self.lane_change_direction
pm.send('pathPlan', plan_send)
dat = messaging.new_message('liveMpc')
dat.liveMpc.x = list(self.mpc_solution[0].x)
dat.liveMpc.y = list(self.mpc_solution[0].y)
dat.liveMpc.psi = list(self.mpc_solution[0].psi)
dat.liveMpc.delta = list(self.mpc_solution[0].delta)
dat.liveMpc.cost = self.mpc_solution[0].cost
pm.send('liveMpc', dat)
msg = messaging_arne.new_message('latControl')
msg.latControl.anglelater = math.degrees(
list(self.mpc_solution[0].delta)[-1])
if not travis:
self.arne_pm.send('latControl', msg)
def thermald_thread():
# prevent LEECO from undervoltage
BATT_PERC_OFF = 25
health_timeout = int(1000 * 2.5 *
DT_TRML) # 2.5x the expected health frequency
# now loop
thermal_sock = messaging.pub_sock('thermal')
health_sock = messaging.sub_sock('health', timeout=health_timeout)
location_sock = messaging.sub_sock('gpsLocation')
ignition = False
fan_speed = 0
count = 0
off_ts = None
started_ts = None
started_seen = False
thermal_status = ThermalStatus.green
thermal_status_prev = ThermalStatus.green
usb_power = True
usb_power_prev = True
network_type = NetworkType.none
network_strength = NetworkStrength.unknown
current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
cpu_temp_filter = FirstOrderFilter(0., CPU_TEMP_TAU, DT_TRML)
health_prev = None
fw_version_match_prev = True
current_connectivity_alert = None
charging_disabled = False
time_valid_prev = True
should_start_prev = False
handle_fan = None
is_uno = False
ts_last_ip = None
ip_addr = '255.255.255.255'
is_uno = (read_tz(29, clip=False) < -1000)
if is_uno or not ANDROID:
handle_fan = handle_fan_uno
else:
setup_eon_fan()
handle_fan = handle_fan_eon
params = Params()
pm = PowerMonitoring()
no_panda_cnt = 0
arne_pm = messaging_arne.PubMaster(['ipAddress'])
while 1:
health = messaging.recv_sock(health_sock, wait=True)
location = messaging.recv_sock(location_sock)
location = location.gpsLocation if location else None
msg = read_thermal()
# clear car params when panda gets connected
if health is not None and health_prev is None:
params.panda_disconnect()
health_prev = health
if health is not None:
usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client
# If we lose connection to the panda, wait 5 seconds before going offroad
if health.health.hwType == log.HealthData.HwType.unknown:
no_panda_cnt += 1
if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML:
if ignition:
cloudlog.error("Lost panda connection while onroad")
ignition = False
else:
no_panda_cnt = 0
ignition = health.health.ignitionLine or health.health.ignitionCan
# Setup fan handler on first connect to panda
if handle_fan is None and health.health.hwType != log.HealthData.HwType.unknown:
is_uno = health.health.hwType == log.HealthData.HwType.uno
if is_uno or not ANDROID:
cloudlog.info("Setting up UNO fan handler")
handle_fan = handle_fan_uno
else:
cloudlog.info("Setting up EON fan handler")
setup_eon_fan()
handle_fan = handle_fan_eon
# Handle disconnect
if health_prev is not None:
if health.health.hwType == log.HealthData.HwType.unknown and \
health_prev.health.hwType != log.HealthData.HwType.unknown:
params.panda_disconnect()
health_prev = health
# get_network_type is an expensive call. update every 10s
if (count % int(10. / DT_TRML)) == 0:
try:
network_type = get_network_type()
network_strength = get_network_strength(network_type)
except Exception:
cloudlog.exception("Error getting network status")
msg.thermal.freeSpace = get_available_percent(default=100.0) / 100.0
msg.thermal.memUsedPercent = int(round(
psutil.virtual_memory().percent))
msg.thermal.cpuPerc = int(round(psutil.cpu_percent()))
msg.thermal.networkType = network_type
msg.thermal.networkStrength = network_strength
msg.thermal.batteryPercent = get_battery_capacity()
msg.thermal.batteryStatus = get_battery_status()
msg.thermal.batteryCurrent = get_battery_current()
msg.thermal.batteryVoltage = get_battery_voltage()
msg.thermal.usbOnline = get_usb_present()
# Fake battery levels on uno for frame
if is_uno:
msg.thermal.batteryPercent = 100
msg.thermal.batteryStatus = "Charging"
# dragonpilot ip Mod
# update ip every 10 seconds
ts = sec_since_boot()
if ts_last_ip is None or ts - ts_last_ip >= 10.:
try:
result = subprocess.check_output(["ifconfig", "wlan0"],
encoding='utf8') # pylint: disable=unexpected-keyword-arg
ip_addr = re.findall(r"inet addr:((\d+\.){3}\d+)",
result)[0][0]
except:
ip_addr = 'N/A'
ts_last_ip = ts
msg2 = messaging_arne.new_message('ipAddress')
msg2.ipAddress.ipAddr = ip_addr
arne_pm.send('ipAddress', msg2)
current_filter.update(msg.thermal.batteryCurrent / 1e6)
# TODO: add car battery voltage check
max_cpu_temp = cpu_temp_filter.update(
max(msg.thermal.cpu0, msg.thermal.cpu1, msg.thermal.cpu2,
msg.thermal.cpu3) / 10.0)
max_comp_temp = max(max_cpu_temp, msg.thermal.mem / 10.,
msg.thermal.gpu / 10.)
bat_temp = msg.thermal.bat / 1000.
if handle_fan is not None:
fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, ignition)
msg.thermal.fanSpeed = fan_speed
# thermal logic with hysterisis
if max_cpu_temp > 107. or bat_temp >= 63.:
# onroad not allowed
thermal_status = ThermalStatus.danger
elif max_comp_temp > 96.0 or bat_temp > 60.: # CPU throttling starts around ~90C
# hysteresis between onroad not allowed and engage not allowed
thermal_status = clip(thermal_status, ThermalStatus.red,
ThermalStatus.danger)
elif max_cpu_temp > 94.0:
# hysteresis between engage not allowed and uploader not allowed
thermal_status = clip(thermal_status, ThermalStatus.yellow,
ThermalStatus.red)
elif max_cpu_temp > 80.0:
# uploader not allowed
thermal_status = ThermalStatus.yellow
elif max_cpu_temp > 75.0:
# hysteresis between uploader not allowed and all good
thermal_status = clip(thermal_status, ThermalStatus.green,
ThermalStatus.yellow)
else:
# all good
thermal_status = ThermalStatus.green
# **** starting logic ****
# Check for last update time and display alerts if needed
now = datetime.datetime.utcnow()
# show invalid date/time alert
time_valid = now.year >= 2019
if time_valid and not time_valid_prev:
params.delete("Offroad_InvalidTime")
if not time_valid and time_valid_prev:
put_nonblocking("Offroad_InvalidTime",
json.dumps(OFFROAD_ALERTS["Offroad_InvalidTime"]))
time_valid_prev = time_valid
# Show update prompt
try:
last_update = datetime.datetime.fromisoformat(
params.get("LastUpdateTime", encoding='utf8'))
except (TypeError, ValueError):
last_update = now
dt = now - last_update
update_failed_count = params.get("UpdateFailedCount")
update_failed_count = 0 if update_failed_count is None else int(
update_failed_count)
if dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1:
if current_connectivity_alert != "expired":
current_connectivity_alert = "expired"
params.delete("Offroad_ConnectivityNeededPrompt")
put_nonblocking(
"Offroad_ConnectivityNeeded",
json.dumps(OFFROAD_ALERTS["Offroad_ConnectivityNeeded"]))
elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0))
if current_connectivity_alert != "prompt" + remaining_time:
current_connectivity_alert = "prompt" + remaining_time
alert_connectivity_prompt = copy.copy(
OFFROAD_ALERTS["Offroad_ConnectivityNeededPrompt"])
alert_connectivity_prompt["text"] += remaining_time + " days."
params.delete("Offroad_ConnectivityNeeded")
put_nonblocking("Offroad_ConnectivityNeededPrompt",
json.dumps(alert_connectivity_prompt))
elif current_connectivity_alert is not None:
current_connectivity_alert = None
params.delete("Offroad_ConnectivityNeeded")
params.delete("Offroad_ConnectivityNeededPrompt")
do_uninstall = params.get("DoUninstall") == b"1"
accepted_terms = params.get("HasAcceptedTerms") == terms_version
completed_training = params.get(
"CompletedTrainingVersion") == training_version
panda_signature = params.get("PandaFirmware", encoding="utf8")
fw_version_match = (panda_signature is None) or (
panda_signature.startswith(FW_SIGNATURE)
) # don't show alert is no panda is connected (None)
should_start = ignition
# with 2% left, we killall, otherwise the phone will take a long time to boot
should_start = should_start and msg.thermal.freeSpace > 0.02
# confirm we have completed training and aren't uninstalling
should_start = should_start and accepted_terms and completed_training and (
not do_uninstall)
# check for firmware mismatch
#should_start = should_start and fw_version_match
# check if system time is valid
should_start = should_start and time_valid
# don't start while taking snapshot
if not should_start_prev:
is_viewing_driver = params.get("IsDriverViewEnabled") == b"1"
is_taking_snapshot = params.get("IsTakingSnapshot") == b"1"
should_start = should_start and (not is_taking_snapshot) and (
not is_viewing_driver)
if fw_version_match and not fw_version_match_prev:
params.delete("Offroad_PandaFirmwareMismatch")
if not fw_version_match and fw_version_match_prev:
put_nonblocking(
"Offroad_PandaFirmwareMismatch",
json.dumps(OFFROAD_ALERTS["Offroad_PandaFirmwareMismatch"]))
# if any CPU gets above 107 or the battery gets above 63, kill all processes
# controls will warn with CPU above 95 or battery above 60
if thermal_status >= ThermalStatus.danger:
should_start = False
if thermal_status_prev < ThermalStatus.danger:
put_nonblocking(
"Offroad_TemperatureTooHigh",
json.dumps(OFFROAD_ALERTS["Offroad_TemperatureTooHigh"]))
else:
if thermal_status_prev >= ThermalStatus.danger:
params.delete("Offroad_TemperatureTooHigh")
if should_start:
if not should_start_prev:
params.delete("IsOffroad")
off_ts = None
if started_ts is None:
started_ts = sec_since_boot()
started_seen = True
os.system(
'echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor'
)
else:
if should_start_prev or (count == 0):
put_nonblocking("IsOffroad", "1")
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()
os.system(
'echo powersave > /sys/class/devfreq/soc:qcom,cpubw/governor'
)
# shutdown if the battery gets lower than 3%, it's discharging, we aren't running for
# more than a minute but we were running
if msg.thermal.batteryPercent < BATT_PERC_OFF and msg.thermal.batteryStatus == "Discharging" and \
started_seen and (sec_since_boot() - off_ts) > 60:
os.system('LD_LIBRARY_PATH="" svc power shutdown')
charging_disabled = check_car_battery_voltage(should_start, health,
charging_disabled, msg)
if msg.thermal.batteryCurrent > 0:
msg.thermal.batteryStatus = "Discharging"
else:
msg.thermal.batteryStatus = "Charging"
msg.thermal.chargingDisabled = charging_disabled
# Offroad power monitoring
pm.calculate(health)
msg.thermal.offroadPowerUsage = pm.get_power_used()
msg.thermal.chargingError = current_filter.x > 0. and msg.thermal.batteryPercent < 90 # if current is positive, then battery is being discharged
msg.thermal.started = started_ts is not None
msg.thermal.startedTs = int(1e9 * (started_ts or 0))
msg.thermal.thermalStatus = thermal_status
thermal_sock.send(msg.to_bytes())
if usb_power_prev and not usb_power:
put_nonblocking(
"Offroad_ChargeDisabled",
json.dumps(OFFROAD_ALERTS["Offroad_ChargeDisabled"]))
elif usb_power and not usb_power_prev:
params.delete("Offroad_ChargeDisabled")
thermal_status_prev = thermal_status
usb_power_prev = usb_power
fw_version_match_prev = fw_version_match
should_start_prev = should_start
print(msg)
# report to server once per minute
if (count % int(60. / DT_TRML)) == 0:
cloudlog.event("STATUS_PACKET",
count=count,
health=(health.to_dict() if health else None),
location=(location.to_dict() if location else None),
thermal=msg.to_dict())
count += 1