def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM, sm_smiskol, df_alert_manager): """Compute conditional state transitions and execute actions on state transitions""" enabled = isEnabled(state) v_cruise_kph_last = v_cruise_kph # if stock cruise is completely disabled, then we can use our own set speed logic if not CP.enableCruise: v_cruise_kph = update_v_cruise(v_cruise_kph, CS.buttonEvents, enabled) elif CP.enableCruise and CS.cruiseState.enabled: v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH # decrease the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state soft_disable_timer = max(0, soft_disable_timer - 1) df_alert = df_alert_manager.update(sm_smiskol) if df_alert is not None: AM.add( frame, 'dfButtonAlert', enabled, extra_text_1=df_alert, extra_text_2='Dynamic follow: {} profile active'.format(df_alert)) # DISABLED if state == State.disabled: if get_events(events, [ET.ENABLE]): if get_events(events, [ET.NO_ENTRY]): for e in get_events(events, [ET.NO_ENTRY]): AM.add(frame, str(e) + "NoEntry", enabled) else: if get_events(events, [ET.PRE_ENABLE]): state = State.preEnabled else: state = State.enabled AM.add(frame, "enable", enabled) v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, v_cruise_kph_last) # ENABLED elif state == State.enabled: if get_events(events, [ET.USER_DISABLE]): state = State.disabled AM.add(frame, "disable", enabled) elif get_events(events, [ET.IMMEDIATE_DISABLE]): state = State.disabled for e in get_events(events, [ET.IMMEDIATE_DISABLE]): AM.add(frame, e, enabled) elif get_events(events, [ET.SOFT_DISABLE]): state = State.softDisabling soft_disable_timer = 300 # 3s for e in get_events(events, [ET.SOFT_DISABLE]): AM.add(frame, e, enabled) # SOFT DISABLING elif state == State.softDisabling: if get_events(events, [ET.USER_DISABLE]): state = State.disabled AM.add(frame, "disable", enabled) elif get_events(events, [ET.IMMEDIATE_DISABLE]): state = State.disabled for e in get_events(events, [ET.IMMEDIATE_DISABLE]): AM.add(frame, e, enabled) elif not get_events(events, [ET.SOFT_DISABLE]): # no more soft disabling condition, so go back to ENABLED state = State.enabled elif get_events(events, [ET.SOFT_DISABLE]) and soft_disable_timer > 0: for e in get_events(events, [ET.SOFT_DISABLE]): AM.add(frame, e, enabled) elif soft_disable_timer <= 0: state = State.disabled # PRE ENABLING elif state == State.preEnabled: if get_events(events, [ET.USER_DISABLE]): state = State.disabled AM.add(frame, "disable", enabled) elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]): state = State.disabled for e in get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]): AM.add(frame, e, enabled) elif not get_events(events, [ET.PRE_ENABLE]): state = State.enabled return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last
def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric): # Given the state, this function returns the actuators # reset actuators to zero actuators = car.CarControl.Actuators.new_message() enabled = isEnabled(state) active = isActive(state) # check if user has interacted with the car driver_engaged = len(CS.buttonEvents) > 0 or \ v_cruise_kph != v_cruise_kph_last or \ CS.steeringPressed # add eventual driver distracted events events = driver_status.update(events, driver_engaged, isActive(state), CS.standstill) # send FCW alert if triggered by planner if plan.fcw: AM.add("fcw", enabled) # ***** state specific actions ***** # DISABLED if state in [State.preEnabled, State.disabled]: LaC.reset() LoC.reset(v_pid=CS.vEgo) # ENABLED or SOFT_DISABLING elif state in [State.enabled, State.softDisabling]: # parse warnings from car specific interface for e in get_events(events, [ET.WARNING]): extra_text = '' if e == "belowSteerSpeed": if is_metric: extra_text = str( int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph" else: extra_text = str( int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph" AM.add(e, enabled, extra_text=extra_text) # *** angle offset learning *** if rk.frame % 5 == 2 and plan.lateralValid: # *** run this at 20hz again *** angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset, PL.PP.c_poly, PL.PP.c_prob, CS.steeringAngle, CS.steeringPressed) # *** gas/brake PID loop *** actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, v_cruise_kph, plan.vTarget, plan.vTargetFuture, plan.aTarget, CP, PL.lead_1) # *** steering PID loop *** actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringPressed, plan.dPoly, angle_offset, VM, PL) # send a "steering required alert" if saturation count has reached the limit if LaC.sat_flag and CP.steerLimitAlert: AM.add("steerSaturated", enabled) # parse permanent warnings to display constantly for e in get_events(events, [ET.PERMANENT]): AM.add(str(e) + "Permanent", enabled) # *** process alerts *** AM.process_alerts(sec_since_boot()) return actuators, v_cruise_kph, driver_status, angle_offset
def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, live100, livempc, AM, driver_status, LaC, LoC, angle_offset, passive): # ***** control the car ***** CC = car.CarControl.new_message() if not passive: CC.enabled = isEnabled(state) CC.actuators = actuators CC.cruiseControl.override = True # always cancel if we have an interceptor CC.cruiseControl.cancel = not CP.enableCruise or ( not isEnabled(state) and CS.cruiseState.enabled) # brake discount removes a sharp nonlinearity brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0)) CC.cruiseControl.speedOverride = float( max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0) CC.cruiseControl.accelOverride = CI.calc_accel_override( CS.aEgo, plan.aTarget, CS.vEgo, plan.vTarget) CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS) CC.hudControl.speedVisible = isEnabled(state) CC.hudControl.lanesVisible = isEnabled(state) CC.hudControl.leadVisible = plan.hasLead CC.hudControl.visualAlert = AM.visual_alert CC.hudControl.audibleAlert = AM.audible_alert # send car controls over can CI.apply(CC, perception_state) # ***** publish state to logger ***** # publish controls state at 100Hz dat = messaging.new_message() dat.init('live100') dat.live100 = { "alertText1": AM.alert_text_1, "alertText2": AM.alert_text_2, "alertSize": AM.alert_size, "alertStatus": AM.alert_status, "alertBlinkingRate": AM.alert_rate, "awarenessStatus": max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0, "driverMonitoringOn": bool(driver_status.monitor_on), "canMonoTimes": list(CS.canMonoTimes), "planMonoTime": plan_ts, "enabled": isEnabled(state), "active": isActive(state), "vEgo": CS.vEgo, "vEgoRaw": CS.vEgoRaw, "angleSteers": CS.steeringAngle, "curvature": VM.calc_curvature(CS.steeringAngle * CV.DEG_TO_RAD, CS.vEgo), "steerOverride": CS.steeringPressed, "state": state, "engageable": not bool(get_events(events, [ET.NO_ENTRY])), "longControlState": LoC.long_control_state, "vPid": float(LoC.v_pid), "vCruise": float(v_cruise_kph), "upAccelCmd": float(LoC.pid.p), "uiAccelCmd": float(LoC.pid.i), "ufAccelCmd": float(LoC.pid.f), "angleSteersDes": float(LaC.angle_steers_des), "upSteer": float(LaC.pid.p), "uiSteer": float(LaC.pid.i), "ufSteer": float(LaC.pid.f), "vTargetLead": float(plan.vTarget), "aTarget": float(plan.aTarget), "jerkFactor": float(plan.jerkFactor), "angleOffset": float(angle_offset), "gpsPlannerActive": plan.gpsPlannerActive, "cumLagMs": -rk.remaining * 1000., } live100.send(dat.to_bytes()) # broadcast carState cs_send = messaging.new_message() cs_send.init('carState') cs_send.carState = CS cs_send.carState.events = events carstate.send(cs_send.to_bytes()) # broadcast carControl cc_send = messaging.new_message() cc_send.init('carControl') cc_send.carControl = CC carcontrol.send(cc_send.to_bytes()) # publish mpc state at 20Hz if hasattr(LaC, 'mpc_updated') and LaC.mpc_updated: dat = messaging.new_message() dat.init('liveMpc') dat.liveMpc.x = list(LaC.mpc_solution[0].x) dat.liveMpc.y = list(LaC.mpc_solution[0].y) dat.liveMpc.psi = list(LaC.mpc_solution[0].psi) dat.liveMpc.delta = list(LaC.mpc_solution[0].delta) dat.liveMpc.cost = LaC.mpc_solution[0].cost livempc.send(dat.to_bytes()) return CC
def update(self, c, can_strings): # ******************* do can recv ******************* canMonoTimes = [] self.cp.update_strings(can_strings) ch_can_valid = self.cp.can_valid self.epas_cp.update_strings(can_strings) epas_can_valid = self.epas_cp.can_valid self.pedal_cp.update_strings(can_strings) pedal_can_valid = self.pedal_cp.can_valid can_rcv_error = not (ch_can_valid and epas_can_valid and pedal_can_valid) self.CS.update(self.cp, self.epas_cp, self.pedal_cp) # create message ret = car.CarState.new_message() ret.canValid = ch_can_valid #and epas_can_valid #and pedal_can_valid # speeds ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) ret.standstill = self.CS.standstill ret.wheelSpeeds.fl = self.CS.v_wheel_fl ret.wheelSpeeds.fr = self.CS.v_wheel_fr ret.wheelSpeeds.rl = self.CS.v_wheel_rl ret.wheelSpeeds.rr = self.CS.v_wheel_rr # gas pedal, we don't use with with interceptor so it's always 0/False ret.gas = self.CS.user_gas if not self.CP.enableGasInterceptor: ret.gasPressed = self.CS.user_gas_pressed else: ret.gasPressed = self.CS.user_gas_pressed # brake pedal ret.brakePressed = False # (self.CS.brake_pressed != 0) and (self.CS.cstm_btns.get_button_status("brake") == 0) # FIXME: read sendcan for brakelights brakelights_threshold = 0.1 ret.brakeLights = bool(self.CS.brake_switch or c.actuators.brake > brakelights_threshold) # steering wheel ret.steeringAngle = self.CS.angle_steers ret.steeringRate = self.CS.angle_steers_rate # gear shifter lever ret.gearShifter = self.CS.gear_shifter ret.steeringTorque = self.CS.steer_torque_driver ret.steeringPressed = self.CS.steer_override # cruise state ret.cruiseState.enabled = True #self.CS.pcm_acc_status != 0 ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS * ( CV.MPH_TO_KPH if self.CS.imperial_speed_units else 1.) ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.speedOffset = 0. ret.cruiseState.standstill = False # TODO: button presses buttonEvents = [] ret.leftBlinker = bool(self.CS.turn_signal_state_left == 1) ret.rightBlinker = bool(self.CS.turn_signal_state_right == 1) ret.doorOpen = not self.CS.door_all_closed ret.seatbeltUnlatched = not self.CS.seatbelt if self.CS.prev_turn_signal_stalk_state != self.CS.turn_signal_stalk_state: if self.CS.turn_signal_stalk_state == 1 or self.CS.prev_turn_signal_stalk_state == 1: be = car.CarState.ButtonEvent.new_message() be.type = 'leftBlinker' be.pressed = self.CS.turn_signal_stalk_state == 1 buttonEvents.append(be) if self.CS.turn_signal_stalk_state == 2 or self.CS.prev_turn_signal_stalk_state == 2: be = car.CarState.ButtonEvent.new_message() be.type = 'rightBlinker' be.pressed = self.CS.turn_signal_stalk_state == 2 buttonEvents.append(be) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_buttons != 0: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: be.type = 'accelCruise' elif but == CruiseButtons.DECEL_SET: be.type = 'decelCruise' elif but == CruiseButtons.CANCEL: be.type = 'cancel' elif but == CruiseButtons.MAIN: be.type = 'altButton3' buttonEvents.append(be) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' be.pressed = bool(self.CS.cruise_buttons) buttonEvents.append(be) ret.buttonEvents = buttonEvents # events events = [] #notification messages for DAS if (not c.enabled) and (self.CC.opState == 2): self.CC.opState = 0 if c.enabled and (self.CC.opState == 0): self.CC.opState = 1 if can_rcv_error: self.can_invalid_count += 1 if self.can_invalid_count >= 100: #BB increased to 100 to see if we still get the can error messages events.append( create_event('invalidGiraffeHonda', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) self.CS.DAS_canErrors = 1 if self.CC.opState == 1: self.CC.opState = 2 else: self.can_invalid_count = 0 if self.CS.steer_error: if not self.CS.enableHSO: events.append( create_event('steerUnavailable', [ET.NO_ENTRY, ET.WARNING])) elif self.CS.steer_warning: if not self.CS.enableHSO: events.append( create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CC.opState == 1: self.CC.opState = 2 if self.CS.brake_error: events.append( create_event( 'brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if self.CC.opState == 1: self.CC.opState = 2 if not ret.gearShifter == 'drive': events.append( create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if c.enabled: self.CC.DAS_222_accCameraBlind = 1 self.CC.warningCounter = 300 self.CC.warningNeeded = 1 if ret.doorOpen: events.append( create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) self.CS.DAS_doorOpen = 1 if self.CC.opState == 1: self.CC.opState = 0 if ret.seatbeltUnlatched: events.append( create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if c.enabled: self.CC.DAS_211_accNoSeatBelt = 1 self.CC.warningCounter = 300 self.CC.warningNeeded = 1 if self.CC.opState == 1: self.CC.opState = 2 if self.CS.esp_disabled: events.append( create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CC.opState == 1: self.CC.opState = 2 if not self.CS.main_on: events.append( create_event('wrongCarMode', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CC.opState == 1: self.CC.opState = 0 if ret.gearShifter == 'reverse': events.append( create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) self.CS.DAS_notInDrive = 1 if self.CC.opState == 1: self.CC.opState = 0 if self.CS.brake_hold: events.append( create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CC.opState == 1: self.CC.opState = 0 if self.CS.park_brake: events.append( create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CC.opState == 1: self.CC.opState = 0 if (not c.enabled) and (self.CC.opState == 1): self.CC.opState = 0 if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) # Standard OP method to disengage: # disable on pedals rising edge or when brake is pressed and speed isn't zero # if (ret.gasPressed and not self.gas_pressed_prev) or \ # (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): # events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) #if (self.CS.cstm_btns.get_button_status("brake")>0): # if ((self.CS.brake_pressed !=0) != self.brake_pressed_prev): #break not canceling when pressed # self.CS.cstm_btns.set_button_status("brake", 2 if self.CS.brake_pressed != 0 else 1) #else: # if ret.brakePressed: # events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low if self.CP.enableCruise and not ret.cruiseState.enabled and c.actuators.brake <= 0.: # non loud alert if cruise disbales below 25mph as expected (+ a little margin) if ret.vEgo < self.CP.minEnableSpeed + 2.: events.append( create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) else: events.append( create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001: events.append(create_event('manualRestart', [ET.WARNING])) cur_time = self.frame * DT_CTRL enable_pressed = False # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type == "altButton3" and not b.pressed: print("enabled pressed at", cur_time) self.last_enable_pressed = cur_time enable_pressed = True # do disable on button down if b.type == "cancel" and b.pressed: events.append(create_event('buttonCancel', [ET.USER_DISABLE])) if self.CP.enableCruise: # KEEP THIS EVENT LAST! send enable event if button is pressed and there are # NO_ENTRY events, so controlsd will display alerts. Also not send enable events # too close in time, so a no_entry will not be followed by another one. # TODO: button press should be the only thing that triggers enble if ((cur_time - self.last_enable_pressed) < 0.2 and # pylint: disable=chained-comparison (cur_time - self.last_enable_sent) > 0.2 and ret.cruiseState.enabled) or \ (enable_pressed and get_events(events, [ET.NO_ENTRY])): if ret.seatbeltUnlatched: self.CC.DAS_211_accNoSeatBelt = 1 self.CC.warningCounter = 300 self.CC.warningNeeded = 1 elif not ret.gearShifter == 'drive': self.CC.DAS_222_accCameraBlind = 1 self.CC.warningCounter = 300 self.CC.warningNeeded = 1 elif not self.CS.apEnabled: self.CC.DAS_206_apUnavailable = 1 self.CC.warningCounter = 300 self.CC.warningNeeded = 1 else: events.append(create_event('buttonEnable', [ET.ENABLE])) self.last_enable_sent = cur_time elif enable_pressed: if ret.seatbeltUnlatched: self.CC.DAS_211_accNoSeatBelt = 1 self.CC.warningCounter = 300 self.CC.warningNeeded = 1 elif not ret.gearShifter == 'drive': self.CC.DAS_222_accCameraBlind = 1 self.CC.warningCounter = 300 self.CC.warningNeeded = 1 elif not self.CS.apEnabled: self.CC.DAS_206_apUnavailable = 1 self.CC.warningCounter = 300 self.CC.warningNeeded = 1 else: events.append(create_event('buttonEnable', [ET.ENABLE])) ret.events = events ret.canMonoTimes = canMonoTimes # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = self.CS.brake_pressed != 0 # cast to reader so it can't be modified return ret.as_reader()
def update(self, c): # ******************* do can recv ******************* can_pub_main = [] canMonoTimes = [] self.CS.update(can_pub_main) # create message ret = car.CarState.new_message() # speeds ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw ret.standstill = self.CS.standstill ret.wheelSpeeds.fl = self.CS.v_wheel_fl ret.wheelSpeeds.fr = self.CS.v_wheel_fr ret.wheelSpeeds.rl = self.CS.v_wheel_rl ret.wheelSpeeds.rr = self.CS.v_wheel_rr # gas pedal ret.gas = self.CS.car_gas / 256.0 if not self.CP.enableGas: ret.gasPressed = self.CS.pedal_gas > 0 else: ret.gasPressed = self.CS.user_gas_pressed # brake pedal ret.brake = self.CS.user_brake ret.brakePressed = self.CS.brake_pressed != 0 # steering wheel ret.steeringAngle = self.CS.angle_steers ret.steeringRate = self.CS.angle_steers_rate # gear shifter lever ret.gearShifter = self.CS.gear_shifter ret.steeringTorque = self.CS.cp.vl[0x18F]['STEER_TORQUE_SENSOR'] ret.steeringPressed = self.CS.steer_override # cruise state ret.cruiseState.enabled = self.CS.pcm_acc_status != 0 ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.speedOffset = self.CS.cruise_speed_offset # TODO: button presses buttonEvents = [] if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: be = car.CarState.ButtonEvent.new_message() be.type = 'leftBlinker' be.pressed = self.CS.left_blinker_on != 0 buttonEvents.append(be) if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: be = car.CarState.ButtonEvent.new_message() be.type = 'rightBlinker' be.pressed = self.CS.right_blinker_on != 0 buttonEvents.append(be) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_buttons != 0: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: be.type = 'accelCruise' elif but == CruiseButtons.DECEL_SET: be.type = 'decelCruise' elif but == CruiseButtons.CANCEL: be.type = 'cancel' elif but == CruiseButtons.MAIN: be.type = 'altButton3' buttonEvents.append(be) if self.CS.cruise_setting != self.CS.prev_cruise_setting: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_setting != 0: be.pressed = True but = self.CS.cruise_setting else: be.pressed = False but = self.CS.prev_cruise_setting if but == 1: be.type = 'altButton1' # TODO: more buttons? buttonEvents.append(be) ret.buttonEvents = buttonEvents # events # TODO: I don't like the way capnp does enums # These strings aren't checked at compile time events = [] if not self.CS.can_valid: self.can_invalid_count += 1 if self.can_invalid_count >= 5: events.append( create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) else: self.can_invalid_count = 0 if self.CS.steer_error: events.append( create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) elif self.CS.steer_not_allowed: events.append( create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) if self.CS.brake_error: events.append( create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not ret.gearShifter == 'drive': events.append( create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.door_all_closed: events.append( create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.seatbelt: events.append( create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CS.esp_disabled: events.append( create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.main_on: events.append( create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gearShifter == 'reverse': events.append( create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CS.brake_hold: events.append( create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CS.park_brake: events.append( create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) # disable on pedals rising edge or when brake is pressed and speed isn't zero if (ret.gasPressed and not self.gas_pressed_prev) or \ (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) #if (ret.brakePressed and ret.vEgo < 0.001) or ret.gasPressed: if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low # TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert if self.CP.enableCruise and not ret.cruiseState.enabled and \ (c.actuators.brake <= 0. or ret.vEgo < 0.3): events.append( create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) cur_time = sec_since_boot() enable_pressed = False # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in ["accelCruise", "decelCruise"] and not b.pressed: print "enabled pressed at", cur_time self.last_enable_pressed = cur_time enable_pressed = True # do disable on button down if b.type == "cancel" and b.pressed: events.append(create_event('buttonCancel', [ET.USER_DISABLE])) if self.CP.enableCruise: # KEEP THIS EVENT LAST! send enable event if button is pressed and there are # NO_ENTRY events, so controlsd will display alerts. Also not send enable events # too close in time, so a no_entry will not be followed by another one. # TODO: button press should be the only thing that triggers enble if ((cur_time - self.last_enable_pressed) < 0.2 and (cur_time - self.last_enable_sent) > 0.2 and ret.cruiseState.enabled) or \ (enable_pressed and get_events(events, [ET.NO_ENTRY])): events.append(create_event('buttonEnable', [ET.ENABLE])) self.last_enable_sent = cur_time elif enable_pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) ret.events = events ret.canMonoTimes = canMonoTimes # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed # cast to reader so it can't be modified #print ret return ret.as_reader()
def update(self, c): # ******************* do can recv ******************* canMonoTimes = [] self.cp.update(int(sec_since_boot() * 1e9), False) self.CS.update(self.cp) # create message ret = car.CarState.new_message() # speeds ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) ret.standstill = self.CS.standstill ret.wheelSpeeds.fl = self.CS.v_wheel_fl ret.wheelSpeeds.fr = self.CS.v_wheel_fr ret.wheelSpeeds.rl = self.CS.v_wheel_rl ret.wheelSpeeds.rr = self.CS.v_wheel_rr # gas pedal ret.gas = self.CS.car_gas / 256.0 if not self.CP.enableGasInterceptor: ret.gasPressed = self.CS.pedal_gas > 0 else: ret.gasPressed = self.CS.user_gas_pressed # brake pedal ret.brake = self.CS.user_brake ret.brakePressed = self.CS.brake_pressed != 0 # FIXME: read sendcan for brakelights brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1 ret.brakeLights = bool(self.CS.brake_switch or c.actuators.brake > brakelights_threshold) # steering wheel ret.steeringAngle = self.CS.angle_steers ret.steeringRate = self.CS.angle_steers_rate # gear shifter lever ret.gearShifter = self.CS.gear_shifter ret.steeringTorque = self.CS.steer_torque_driver ret.steeringPressed = self.CS.steer_override # cruise state ret.cruiseState.enabled = self.CS.pcm_acc_status != 0 ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.speedOffset = self.CS.cruise_speed_offset ret.cruiseState.standstill = False ret.readdistancelines = self.CS.read_distance_lines # TODO: button presses buttonEvents = [] ret.leftBlinker = bool(self.CS.left_blinker_on) ret.rightBlinker = bool(self.CS.right_blinker_on) ret.doorOpen = not self.CS.door_all_closed ret.seatbeltUnlatched = not self.CS.seatbelt if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: be = car.CarState.ButtonEvent.new_message() be.type = 'leftBlinker' be.pressed = self.CS.left_blinker_on != 0 buttonEvents.append(be) if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: be = car.CarState.ButtonEvent.new_message() be.type = 'rightBlinker' be.pressed = self.CS.right_blinker_on != 0 buttonEvents.append(be) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_buttons != 0: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: be.type = 'accelCruise' elif but == CruiseButtons.DECEL_SET: be.type = 'decelCruise' elif but == CruiseButtons.CANCEL: be.type = 'cancel' elif but == CruiseButtons.MAIN: be.type = 'altButton3' buttonEvents.append(be) if self.CS.cruise_setting != self.CS.prev_cruise_setting: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_setting != 0: be.pressed = True but = self.CS.cruise_setting else: be.pressed = False but = self.CS.prev_cruise_setting if but == 1: be.type = 'altButton1' # TODO: more buttons? buttonEvents.append(be) ret.buttonEvents = buttonEvents ret.gasbuttonstatus = self.CS.cstm_btns.get_button_status("gas") # events # TODO: I don't like the way capnp does enums # These strings aren't checked at compile time events = [] if not self.CS.can_valid: self.can_invalid_count += 1 if self.can_invalid_count >= 5: events.append( create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) else: self.can_invalid_count = 0 if self.CS.steer_error: events.append( create_event( 'steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) elif self.CS.steer_warning: events.append(create_event('steerTempUnavailable', [ET.WARNING])) if self.CS.brake_error: events.append( create_event( 'brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if not ret.gearShifter == 'drive': events.append( create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.doorOpen: events.append( create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.seatbeltUnlatched: events.append( create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CS.esp_disabled: events.append( create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.main_on: events.append( create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gearShifter == 'reverse': events.append( create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CS.brake_hold and self.CS.CP.carFingerprint not in HONDA_BOSCH: events.append( create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CS.park_brake: events.append( create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) # disable on pedals rising edge or when brake is pressed and speed isn't zero if (ret.gasPressed and not self.gas_pressed_prev) or \ (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low if self.CP.enableCruise and not ret.cruiseState.enabled and c.actuators.brake <= 0.: # non loud alert if cruise disbales below 25mph as expected (+ a little margin) if ret.vEgo < self.CP.minEnableSpeed + 2.: events.append( create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) else: events.append( create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001: events.append(create_event('manualRestart', [ET.WARNING])) cur_time = sec_since_boot() enable_pressed = False # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in ["accelCruise", "decelCruise"] and not b.pressed: self.last_enable_pressed = cur_time enable_pressed = True # do disable on button down if b.type == "cancel" and b.pressed: events.append(create_event('buttonCancel', [ET.USER_DISABLE])) if self.CP.enableCruise: # KEEP THIS EVENT LAST! send enable event if button is pressed and there are # NO_ENTRY events, so controlsd will display alerts. Also not send enable events # too close in time, so a no_entry will not be followed by another one. # TODO: button press should be the only thing that triggers enble if ((cur_time - self.last_enable_pressed) < 0.2 and (cur_time - self.last_enable_sent) > 0.2 and ret.cruiseState.enabled) or \ (enable_pressed and get_events(events, [ET.NO_ENTRY])): events.append(create_event('buttonEnable', [ET.ENABLE])) self.last_enable_sent = cur_time elif enable_pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) ret.events = events ret.canMonoTimes = canMonoTimes # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed # cast to reader so it can't be modified return ret.as_reader()
def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, driver_status, LaC, LoC, VM, angle_model_bias, passive, is_metric, cal_perc): """Given the state, this function returns an actuators packet""" actuators = car.CarControl.Actuators.new_message() enabled = isEnabled(state) active = isActive(state) # check if user has interacted with the car driver_engaged = len(CS.buttonEvents) > 0 or \ v_cruise_kph != v_cruise_kph_last or \ CS.steeringPressed # add eventual driver distracted events events = driver_status.update(events, driver_engaged, isActive(state), CS.standstill) # send FCW alert if triggered by planner if plan.fcw: AM.add("fcw", enabled) # State specific actions if state in [State.preEnabled, State.disabled]: LaC.reset() LoC.reset(v_pid=CS.vEgo) elif state in [State.enabled, State.softDisabling]: # parse warnings from car specific interface for e in get_events(events, [ET.WARNING]): extra_text = "" if e == "belowSteerSpeed": if is_metric: extra_text = str( int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph" else: extra_text = str( int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph" AM.add(e, enabled, extra_text_2=extra_text) # Run angle offset learner at 20 Hz if rk.frame % 5 == 2: angle_model_bias = learn_angle_model_bias( active, CS.vEgo, angle_model_bias, path_plan.cPoly, path_plan.cProb, CS.steeringAngle, CS.steeringPressed) cur_time = sec_since_boot() # TODO: This won't work in replay mpc_time = plan.l20MonoTime / 1e9 _DT = 0.01 # 100Hz dt = min( cur_time - mpc_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps a_acc_sol = plan.aStart + (dt / _DT_MPC) * (plan.aTarget - plan.aStart) v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0 # Gas/Brake PID loop actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP) # Steering PID loop and lateral MPC actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringPressed, CP, VM, path_plan) # Send a "steering required alert" if saturation count has reached the limit if LaC.sat_flag and CP.steerLimitAlert: AM.add("steerSaturated", enabled) # Parse permanent warnings to display constantly for e in get_events(events, [ET.PERMANENT]): extra_text_1, extra_text_2 = "", "" if e == "calibrationIncomplete": extra_text_1 = str(cal_perc) + "%" if is_metric: extra_text_2 = str(int(round( Filter.MIN_SPEED * CV.MS_TO_KPH))) + " kph" else: extra_text_2 = str(int(round( Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph" AM.add(str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2) AM.process_alerts(sec_since_boot()) return actuators, v_cruise_kph, driver_status, angle_model_bias, v_acc_sol, a_acc_sol
def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, awareness_status, PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle): # Given the state, this function returns the actuators # reset actuators to zero actuators = car.CarControl.Actuators.new_message() enabled = isEnabled(state) active = isActive(state) for b in CS.buttonEvents: # button presses for rear view if b.type == "leftBlinker" or b.type == "rightBlinker": if b.pressed and rear_view_allowed: rear_view_toggle = True else: rear_view_toggle = False if b.type == "altButton1" and b.pressed: rear_view_toggle = not rear_view_toggle # send FCW alert if triggered by planner if plan.fcw: AM.add("fcw", enabled) # ***** state specific actions ***** # DISABLED if state in [State.preEnabled, State.disabled]: LaC.reset() LoC.reset(v_pid=CS.vEgo) # ENABLED or SOFT_DISABLING elif state in [State.enabled, State.softDisabling]: # decrease awareness status awareness_status -= 0.01 / (AWARENESS_TIME) if awareness_status <= 0.: AM.add("driverDistracted", enabled) elif awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \ awareness_status >= (AWARENESS_PRE_TIME - 4.) / AWARENESS_TIME: AM.add("preDriverDistracted", enabled) # parse warnings from car specific interface for e in get_events(events, [ET.WARNING]): AM.add(e, enabled) # *** angle offset learning *** if rk.frame % 5 == 2 and plan.lateralValid: # *** run this at 20hz again *** angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset, PL.PP.c_poly, PL.PP.c_prob, CS.steeringAngle, CS.steeringPressed) # *** gas/brake PID loop *** actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, v_cruise_kph, plan.vTarget, plan.vTargetFuture, plan.aTarget, CP, PL.lead_1) # *** steering PID loop *** actuators.steer = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringPressed, plan.dPoly, angle_offset, VM, PL) # send a "steering required alert" if saturation count has reached the limit if LaC.sat_flag and CP.steerLimitAlert: AM.add("steerSaturated", enabled) if CP.enableCruise and CS.cruiseState.enabled: v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH # reset conditions for the 6 minutes timout if CS.buttonEvents or \ v_cruise_kph != v_cruise_kph_last or \ CS.steeringPressed or \ state in [State.preEnabled, State.disabled]: awareness_status = 1. # parse permanent warnings to display constantly for e in get_events(events, [ET.PERMANENT]): AM.add(str(e) + "Permanent", enabled) # *** process alerts *** AM.process_alerts(sec_since_boot()) return actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle
def update(self, c, can_strings): # ******************* do can recv ******************* self.cp.update_strings(can_strings) self.cp_cam.update_strings(can_strings) ret = self.CS.update(self.cp, self.cp_cam) ret.canValid = self.cp.can_valid and self.cp_cam.can_valid ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo) # FIXME: read sendcan for brakelights brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1 ret.brakeLights = bool(self.CS.brake_switch or c.actuators.brake > brakelights_threshold) buttonEvents = [] if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = ButtonType.unknown if self.CS.cruise_buttons != 0: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: be.type = ButtonType.accelCruise elif but == CruiseButtons.DECEL_SET: be.type = ButtonType.decelCruise elif but == CruiseButtons.CANCEL: be.type = ButtonType.cancel elif but == CruiseButtons.MAIN: be.type = ButtonType.altButton3 buttonEvents.append(be) if self.CS.cruise_setting != self.CS.prev_cruise_setting: be = car.CarState.ButtonEvent.new_message() be.type = ButtonType.unknown if self.CS.cruise_setting != 0: be.pressed = True but = self.CS.cruise_setting else: be.pressed = False but = self.CS.prev_cruise_setting if but == 1: be.type = ButtonType.altButton1 # TODO: more buttons? buttonEvents.append(be) ret.buttonEvents = buttonEvents # events events = self.create_common_events(ret) if self.CS.brake_error: events.append( create_event( 'brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if self.CS.brake_hold and self.CS.CP.carFingerprint not in HONDA_BOSCH: events.append( create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CS.park_brake: events.append( create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) # disable on pedals rising edge or when brake is pressed and speed isn't zero if (ret.gasPressed and not self.gas_pressed_prev) or \ (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low if self.CP.enableCruise and not ret.cruiseState.enabled and ( c.actuators.brake <= 0. or not self.CP.openpilotLongitudinalControl): # non loud alert if cruise disbales below 25mph as expected (+ a little margin) if ret.vEgo < self.CP.minEnableSpeed + 2.: events.append( create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) else: events.append( create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001: events.append(create_event('manualRestart', [ET.WARNING])) cur_time = self.frame * DT_CTRL enable_pressed = False # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in [ButtonType.accelCruise, ButtonType.decelCruise ] and not b.pressed: self.last_enable_pressed = cur_time enable_pressed = True # do disable on button down if b.type == "cancel" and b.pressed: events.append(create_event('buttonCancel', [ET.USER_DISABLE])) if self.CP.enableCruise: # KEEP THIS EVENT LAST! send enable event if button is pressed and there are # NO_ENTRY events, so controlsd will display alerts. Also not send enable events # too close in time, so a no_entry will not be followed by another one. # TODO: button press should be the only thing that triggers enable if ((cur_time - self.last_enable_pressed) < 0.2 and (cur_time - self.last_enable_sent) > 0.2 and ret.cruiseState.enabled) or \ (enable_pressed and get_events(events, [ET.NO_ENTRY])): events.append(create_event('buttonEnable', [ET.ENABLE])) self.last_enable_sent = cur_time elif enable_pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) ret.events = events # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed self.CS.out = ret.as_reader() return self.CS.out
def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc): """Given the state, this function returns an actuators packet""" actuators = car.CarControl.Actuators.new_message() enabled = isEnabled(state) active = isActive(state) # check if user has interacted with the car driver_engaged = len(CS.buttonEvents) > 0 or \ v_cruise_kph != v_cruise_kph_last or \ CS.steeringPressed # add eventual driver distracted events events = driver_status.update(events, driver_engaged, isActive(state), CS.standstill) # send FCW alert if triggered by planner if plan.fcw: AM.add("fcw", enabled) # State specific actions if state in [State.preEnabled, State.disabled]: LaC.reset() LoC.reset(v_pid=CS.vEgo) elif state in [State.enabled, State.softDisabling]: # parse warnings from car specific interface for e in get_events(events, [ET.WARNING]): extra_text = "" if e == "belowSteerSpeed": if is_metric: extra_text = str( int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph" else: extra_text = str( int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph" AM.add(e, enabled, extra_text_2=extra_text) # Run angle offset learner at 20 Hz if rk.frame % 5 == 2 and plan.lateralValid: angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset, PL.PP.c_poly, PL.PP.c_prob, CS.steeringAngle, CS.steeringPressed) if CS.gasbuttonstatus == 0: CP.gasMaxV = [0.2, 0.2, 0.2] else: CP.gasMaxV = [0.3, 0.7, 0.9] # Gas/Brake PID loop actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, v_cruise_kph, plan.vTarget, plan.vTargetFuture, plan.aTarget, CP, PL.lead_1) # Steering PID loop and lateral MPC actuators.steer, actuators.steerAngle = LaC.update( active, CS.vEgo, CS.steeringAngle, CS.steeringPressed, plan.dPoly, angle_offset, CP, VM, PL, CS.blindspot, CS.leftBlinker, CS.rightBlinker) #BB added for ALCA support #CS.pid = LaC.pid # Send a "steering required alert" if saturation count has reached the limit if LaC.sat_flag and CP.steerLimitAlert: AM.add("steerSaturated", enabled) # Parse permanent warnings to display constantly for e in get_events(events, [ET.PERMANENT]): extra_text_1, extra_text_2 = "", "" if e == "calibrationIncomplete": extra_text_1 = str(cal_perc) + "%" if is_metric: extra_text_2 = str(int(round( Filter.MIN_SPEED * CV.MS_TO_KPH))) + " kph" else: extra_text_2 = str(int(round( Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph" AM.add(str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2) AM.process_alerts(sec_since_boot()) return actuators, v_cruise_kph, driver_status, angle_offset
def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM): # compute conditional state transitions and execute actions on state transitions enabled = isEnabled(state) # handle button presses. TODO: this should be in state_control, but a decelCruise press # would have the effect of both enabling and changing speed is checked after the state transition v_cruise_kph_last = v_cruise_kph for b in CS.buttonEvents: if not CP.enableCruise and enabled and not b.pressed: if b.type == "accelCruise": v_cruise_kph -= (v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA elif b.type == "decelCruise": v_cruise_kph -= (v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX) # decrease the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state soft_disable_timer = max(0, soft_disable_timer - 1) # ***** handle state transitions ***** # DISABLED if state == State.disabled: if get_events(events, [ET.ENABLE]): if get_events(events, [ET.NO_ENTRY]): for e in get_events(events, [ET.NO_ENTRY]): AM.add(str(e) + "NoEntry", enabled) else: if get_events(events, [ET.PRE_ENABLE]): state = State.preEnabled else: state = State.enabled AM.add("enable", enabled) # on activation, let's always set v_cruise from where we are, even if PCM ACC is active v_cruise_kph = int( round(max(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN))) # ENABLED elif state == State.enabled: if get_events(events, [ET.USER_DISABLE]): state = State.disabled AM.add("disable", enabled) elif get_events(events, [ET.IMMEDIATE_DISABLE]): state = State.disabled for e in get_events(events, [ET.IMMEDIATE_DISABLE]): AM.add(e, enabled) elif get_events(events, [ET.SOFT_DISABLE]): state = State.softDisabling soft_disable_timer = 300 # 3s TODO: use rate for e in get_events(events, [ET.SOFT_DISABLE]): AM.add(e, enabled) # SOFT DISABLING elif state == State.softDisabling: if get_events(events, [ET.USER_DISABLE]): state = State.disabled AM.add("disable", enabled) elif get_events(events, [ET.IMMEDIATE_DISABLE]): state = State.disabled for e in get_events(events, [ET.IMMEDIATE_DISABLE]): AM.add(e, enabled) elif not get_events(events, [ET.SOFT_DISABLE]): # no more soft disabling condition, so go back to ENABLED state = State.enabled elif soft_disable_timer <= 0: state = State.disabled # PRE ENABLING elif state == State.preEnabled: if get_events(events, [ET.USER_DISABLE]): state = State.disabled AM.add("disable", enabled) elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]): state = State.disabled for e in get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]): AM.add(e, enabled) elif not get_events(events, [ET.PRE_ENABLE]): state = State.enabled return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last
def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame, is_ldw_enabled, can_error_counter): """Send actuators and hud commands to the car, send controlsstate and MPC logging""" global trace1 CC = car.CarControl.new_message() CC.enabled = isEnabled(state) CC.actuators = actuators CC.cruiseControl.override = True CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled) # Some override values for Honda brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0) ) # brake discount removes a sharp nonlinearity CC.cruiseControl.speedOverride = float( max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0) CC.cruiseControl.accelOverride = CI.calc_accel_override( CS.aEgo, sm['plan'].aTarget, CS.vEgo, sm['plan'].vTarget) CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS) CC.hudControl.speedVisible = isEnabled(state) CC.hudControl.lanesVisible = isEnabled(state) CC.hudControl.leadVisible = sm['plan'].hasLead right_lane_visible = sm['pathPlan'].rProb left_lane_visible = sm['pathPlan'].lProb #lane_dPoly = sm['pathPlan'].dPoly #lane_width = sm['pathPlan'].laneWidth #lane_lPoly = sm['pathPlan'].lPoly #lane_rPloy = sm['pathPlan'].rPoly #str_dPoly = 'P=' #for x in lane_dPoly: # str_dPoly += '{:.5f},'.format( x ) #str_log = 'L:{:.1f}/{:.1f} w:{:.1f} {}'.format( left_lane_visible, right_lane_visible, lane_width, str_dPoly ) #trace1.printf( str_log ) #traceCS.add( str_log ) lane_visible = right_lane_visible + left_lane_visible if lane_visible > 1: CC.hudControl.rightLaneVisible = True CC.hudControl.leftLaneVisible = True else: CC.hudControl.rightLaneVisible = bool(right_lane_visible > 0.4) CC.hudControl.leftLaneVisible = bool(left_lane_visible > 0.4) AM.process_alerts(sm.frame) CC.hudControl.visualAlert = AM.visual_alert # steer hand check. if not read_only: # send car controls over can can_sends = CI.apply(CC, sm, LaC) pm.send( 'sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) force_decel = sm['dMonitoringState'].awarenessStatus < 0. # controlsState dat = messaging.new_message() dat.init('controlsState') dat.valid = CS.canValid dat.controlsState = { "alertText1": AM.alert_text_1, "alertText2": AM.alert_text_2, "alertSize": AM.alert_size, "alertStatus": AM.alert_status, "alertBlinkingRate": AM.alert_rate, "alertType": AM.alert_type, "alertSound": AM.audible_alert, "driverMonitoringOn": sm['dMonitoringState'].faceDetected, "canMonoTimes": list(CS.canMonoTimes), "planMonoTime": sm.logMonoTime['plan'], "pathPlanMonoTime": sm.logMonoTime['pathPlan'], "enabled": isEnabled(state), "active": isActive(state), "vEgo": CS.vEgo, "vEgoRaw": CS.vEgoRaw, "angleSteers": CS.steeringAngle, "curvature": VM.calc_curvature( (CS.steeringAngle - sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD, CS.vEgo), "steerOverride": CS.steeringPressed, "state": state, "engageable": not bool(get_events(events, [ET.NO_ENTRY])), "longControlState": LoC.long_control_state, "vPid": float(LoC.v_pid), "vCruise": float(v_cruise_kph), "upAccelCmd": float(LaC.pid.p), "uiAccelCmd": float(LaC.pid.i), "ufAccelCmd": float(LaC.pid.f), "angleSteersDes": float(LaC.angle_steers_des), "vTargetLead": float(v_acc), "aTarget": float(a_acc), "jerkFactor": float(sm['plan'].jerkFactor), "gpsPlannerActive": sm['plan'].gpsPlannerActive, "vCurvature": float(LaC.model_sum), # sm['plan'].vCurvature, "decelForModel": sm['plan'].longitudinalPlanSource == log.Plan.LongitudinalPlanSource.model, "cumLagMs": -rk.remaining * 1000., "startMonoTime": int(start_time * 1e9), "mapValid": sm['plan'].mapValid, "forceDecel": bool(force_decel), "canErrorCounter": trace1.cruise_set_mode, "alertTextMsg1": str(trace1.global_alertTextMsg1), "alertTextMsg2": str(trace1.global_alertTextMsg2), } if CP.lateralTuning.which() == 'pid': dat.controlsState.lateralControlState.pidState = lac_log elif CP.lateralTuning.which() == 'lqr': dat.controlsState.lateralControlState.lqrState = lac_log elif CP.lateralTuning.which() == 'indi': dat.controlsState.lateralControlState.indiState = lac_log pm.send('controlsState', dat) # carState cs_send = messaging.new_message() cs_send.init('carState') cs_send.valid = CS.canValid cs_send.carState = CS cs_send.carState.events = events pm.send('carState', cs_send) # carEvents - logged every second or on change events_bytes = events_to_bytes(events) if (sm.frame % int(1. / DT_CTRL) == 0) or (events_bytes != events_prev): ce_send = messaging.new_message() ce_send.init('carEvents', len(events)) ce_send.carEvents = events pm.send('carEvents', ce_send) # carParams - logged every 50 seconds (> 1 per segment) if (sm.frame % int(50. / DT_CTRL) == 0): cp_send = messaging.new_message() cp_send.init('carParams') cp_send.carParams = CP pm.send('carParams', cp_send) # carControl cc_send = messaging.new_message() cc_send.init('carControl') cc_send.valid = CS.canValid cc_send.carControl = CC pm.send('carControl', cc_send) return CC, events_bytes
def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, sm, poller, arne182Status): """Given the state, this function returns an actuators packet""" actuators = car.CarControl.Actuators.new_message() enabled = isEnabled(state) active = isActive(state) # check if user has interacted with the car driver_engaged = len(CS.buttonEvents) > 0 or \ v_cruise_kph != v_cruise_kph_last or \ CS.steeringPressed # add eventual driver distracted events events = driver_status.update(events, driver_engaged, isActive(state), CS.standstill) # send FCW alert if triggered by planner if plan.fcw: AM.add(frame, "fcw", enabled) # State specific actions if state in [State.preEnabled, State.disabled]: LaC.reset() LoC.reset(v_pid=CS.vEgo) elif state in [State.enabled, State.softDisabling]: # parse warnings from car specific interface for e in get_events(events, [ET.WARNING]): extra_text = "" if e == "belowSteerSpeed": if is_metric: extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph" else: extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph" AM.add(frame, e, enabled, extra_text_2=extra_text) plan_age = DT_CTRL * (frame - rcv_frame['plan']) dt = min(plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL # no greater than dt mpc + dt, to prevent too high extraps a_acc_sol = plan.aStart + (dt / LON_MPC_STEP) * (plan.aTarget - plan.aStart) v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0 try: gasinterceptor = CP.enableGasInterceptor except AttributeError: gasinterceptor = False # Gas/Brake PID loop try: leadOne = sm['radarState'].leadOne except KeyError: leadOne = None gasstatus = None for socket, _ in poller.poll(0): if socket is arne182Status: gasstatus = arne182.Arne182Status.from_bytes(socket.recv()) if gasstatus is None: gasbuttonstatus = 0 else: gasbuttonstatus = gasstatus.gasbuttonstatus actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP, gasinterceptor, gasbuttonstatus, plan.decelForTurn, plan.longitudinalPlanSource, leadOne, CS.gasPressed) # Steering PID loop and lateral MPC actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CP, path_plan) # Send a "steering required alert" if saturation count has reached the limit if LaC.sat_flag and CP.steerLimitAlert: AM.add(frame, "steerSaturated", enabled) # Parse permanent warnings to display constantly for e in get_events(events, [ET.PERMANENT]): extra_text_1, extra_text_2 = "", "" if e == "calibrationIncomplete": extra_text_1 = str(cal_perc) + "%" if is_metric: extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_KPH))) + " kph" else: extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph" AM.add(frame, str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2) AM.process_alerts(frame) return actuators, v_cruise_kph, driver_status, v_acc_sol, a_acc_sol, lac_log
def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_status, PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle): # Given the state, this function returns the actuators # reset actuators to zero actuators = car.CarControl.Actuators.new_message() enabled = isEnabled(state) active = isActive(state) for b in CS.buttonEvents: # any button event resets awarenesss_status awareness_status = 1. # button presses for rear view if b.type == "leftBlinker" or b.type == "rightBlinker": if b.pressed and rear_view_allowed: rear_view_toggle = True else: rear_view_toggle = False if b.type == "altButton1" and b.pressed: rear_view_toggle = not rear_view_toggle # send FCW alert if triggered by planner if plan.fcw: AM.add("fcw", enabled) # ***** state specific actions ***** # DISABLED if state in [State.PRE_ENABLED, State.DISABLED]: awareness_status = 1. LaC.reset() LoC.reset(v_pid=CS.vEgo) # ENABLED or SOFT_DISABLING elif state in [State.ENABLED, State.SOFT_DISABLING]: if CS.steeringPressed: # reset awareness status on steering awareness_status = 1.0 # 6 minutes driver you're on awareness_status -= 0.01 / (AWARENESS_TIME) if awareness_status <= 0.: AM.add("driverDistracted", enabled) elif awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \ awareness_status >= (AWARENESS_PRE_TIME - 0.1) / AWARENESS_TIME: AM.add("preDriverDistracted", enabled) # parse warnings from car specific interface for e in get_events(events, [ET.WARNING]): AM.add(e, enabled) # if user is not responsive to awareness alerts, then start a smooth deceleration if awareness_status < -0.: plan.aTargetMax = min(plan.aTargetMax, AWARENESS_DECEL) plan.aTargetMin = min(plan.aTargetMin, plan.aTargetMax) # *** angle offset learning *** if rk.frame % 5 == 2 and plan.lateralValid: # *** run this at 20hz again *** angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset, PL.PP.c_poly, PL.PP.c_prob, LaC.y_des, CS.steeringPressed) # *** gas/brake PID loop *** actuators.gas, actuators.brake = LoC.update( active, CS.vEgo, CS.brakePressed, v_cruise_kph, plan.vTarget, [plan.aTargetMin, plan.aTargetMax], plan.jerkFactor, CP) # *** steering PID loop *** actuators.steer = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringPressed, plan.dPoly, angle_offset, VM, PL) # send a "steering required alert" if saturation count has reached the limit if LaC.sat_flag: AM.add("steerSaturated", enabled) if CP.enableCruise and CS.cruiseState.enabled: v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH # *** process alerts *** AM.process_alerts(sec_since_boot()) return actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle
def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame): """Given the state, this function returns an actuators packet""" actuators = car.CarControl.Actuators.new_message() enabled = isEnabled(state) active = isActive(state) # check if user has interacted with the car driver_engaged = len(CS.buttonEvents) > 0 or \ v_cruise_kph != v_cruise_kph_last or \ CS.steeringPressed if CS.leftBlinker or CS.rightBlinker: last_blinker_frame = frame # add eventual driver distracted events events = driver_status.update(events, driver_engaged, isActive(state), CS.standstill) if plan.fcw: # send FCW alert if triggered by planner AM.add(frame, "fcw", enabled) elif CS.stockFcw: # send a silent alert when stock fcw triggers, since the car is already beeping AM.add(frame, "fcwStock", enabled) # State specific actions if state in [State.preEnabled, State.disabled]: LaC.reset() LoC.reset(v_pid=CS.vEgo) elif state in [State.enabled, State.softDisabling]: # parse warnings from car specific interface for e in get_events(events, [ET.WARNING]): extra_text = "" if e == "belowSteerSpeed": if is_metric: extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph" else: extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph" AM.add(frame, e, enabled, extra_text_2=extra_text) plan_age = DT_CTRL * (frame - rcv_frame['plan']) dt = min(plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL # no greater than dt mpc + dt, to prevent too high extraps a_acc_sol = plan.aStart + (dt / LON_MPC_STEP) * (plan.aTarget - plan.aStart) v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0 # Gas/Brake PID loop actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP) # Steering PID loop and lateral MPC actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CS.steeringRateLimited, CP, path_plan) # Send a "steering required alert" if saturation count has reached the limit if lac_log.saturated and not CS.steeringPressed: # Check if we deviated from the path left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.1 right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.1 if left_deviation or right_deviation: AM.add(frame, "steerSaturated", enabled) # Parse permanent warnings to display constantly for e in get_events(events, [ET.PERMANENT]): extra_text_1, extra_text_2 = "", "" if e == "calibrationIncomplete": extra_text_1 = str(cal_perc) + "%" if is_metric: extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_KPH))) + " kph" else: extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph" AM.add(frame, str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2) return actuators, v_cruise_kph, driver_status, v_acc_sol, a_acc_sol, lac_log, last_blinker_frame
def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, carevents, carparams, controlsstate, sendcan, AM, driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev): """Send actuators and hud commands to the car, send controlsstate and MPC logging""" try: CC = car.CarControl.new_message() CC.enabled = isEnabled(state) CC.actuators = actuators CC.cruiseControl.override = True CC.cruiseControl.cancel = not CP.enableCruise or ( not isEnabled(state) and CS.cruiseState.enabled) # Some override values for Honda brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0) ) # brake discount removes a sharp nonlinearity CC.cruiseControl.speedOverride = float( max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0) CC.cruiseControl.accelOverride = CI.calc_accel_override( CS.aEgo, sm['plan'].aTarget, CS.vEgo, sm['plan'].vTarget) CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS) CC.hudControl.speedVisible = isEnabled(state) CC.hudControl.lanesVisible = isEnabled(state) CC.hudControl.leadVisible = sm['plan'].hasLead right_lane_visible = sm['pathPlan'].rProb > 0.5 left_lane_visible = sm['pathPlan'].lProb > 0.5 CC.hudControl.rightLaneVisible = bool(right_lane_visible) CC.hudControl.leftLaneVisible = bool(left_lane_visible) blinker = CS.leftBlinker or CS.rightBlinker ldw_allowed = CS.vEgo > 12.5 and not blinker if len(list(sm['pathPlan'].rPoly)) == 4: CC.hudControl.rightLaneDepart = bool( ldw_allowed and sm['pathPlan'].rPoly[3] > -(1 + CAMERA_OFFSET) and right_lane_visible) if len(list(sm['pathPlan'].lPoly)) == 4: CC.hudControl.leftLaneDepart = bool(ldw_allowed and sm['pathPlan'].lPoly[3] < (1 - CAMERA_OFFSET) and left_lane_visible) CC.hudControl.visualAlert = AM.visual_alert CC.hudControl.audibleAlert = AM.audible_alert if not read_only: # send car controls over can can_sends = CI.apply(CC) sendcan.send( can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) force_decel = driver_status.awareness < 0. # controlsState dat = messaging.new_message() dat.init('controlsState') dat.valid = CS.canValid dat.controlsState = { "alertText1": AM.alert_text_1, "alertText2": AM.alert_text_2, "alertSize": AM.alert_size, "alertStatus": AM.alert_status, "alertBlinkingRate": AM.alert_rate, "alertType": AM.alert_type, "alertSound": "", # no EON sounds yet "awarenessStatus": max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0, "driverMonitoringOn": bool(driver_status.monitor_on and driver_status.face_detected), "canMonoTimes": list(CS.canMonoTimes), "planMonoTime": sm.logMonoTime['plan'], "pathPlanMonoTime": sm.logMonoTime['pathPlan'], "enabled": isEnabled(state), "active": isActive(state), "vEgo": CS.vEgo, "vEgoRaw": CS.vEgoRaw, "angleSteers": CS.steeringAngle, "curvature": VM.calc_curvature((CS.steeringAngle - sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD, CS.vEgo), "steerOverride": CS.steeringPressed, "state": state, "engageable": not bool(get_events(events, [ET.NO_ENTRY])), "longControlState": LoC.long_control_state, "vPid": float(LoC.v_pid), "vCruise": float(v_cruise_kph), "upAccelCmd": float(LoC.pid.p), "uiAccelCmd": float(LoC.pid.i), "ufAccelCmd": float(LoC.pid.f), "angleSteersDes": float(LaC.angle_steers_des), "vTargetLead": float(v_acc), "aTarget": float(a_acc), "jerkFactor": float(sm['plan'].jerkFactor), "angleModelBias": 0., "gpsPlannerActive": sm['plan'].gpsPlannerActive, "vCurvature": sm['plan'].vCurvature, "decelForModel": sm['plan'].longitudinalPlanSource == log.Plan.LongitudinalPlanSource.model, "cumLagMs": -rk.remaining * 1000., "startMonoTime": int(start_time * 1e9), "mapValid": sm['plan'].mapValid, "forceDecel": bool(force_decel), } if CP.lateralTuning.which() == 'pid': dat.controlsState.lateralControlState.pidState = lac_log else: dat.controlsState.lateralControlState.indiState = lac_log controlsstate.send(dat.to_bytes()) # carState cs_send = messaging.new_message() cs_send.init('carState') cs_send.valid = CS.canValid cs_send.carState = CS cs_send.carState.events = events carstate.send(cs_send.to_bytes()) # carEvents - logged every second or on change events_bytes = events_to_bytes(events) if (sm.frame % int(1. / DT_CTRL) == 0) or (events_bytes != events_prev): ce_send = messaging.new_message() ce_send.init('carEvents', len(events)) ce_send.carEvents = events carevents.send(ce_send.to_bytes()) # carParams - logged every 50 seconds (> 1 per segment) if (sm.frame % int(50. / DT_CTRL) == 0): cp_send = messaging.new_message() cp_send.init('carParams') cp_send.carParams = CP carparams.send(cp_send.to_bytes()) # carControl cc_send = messaging.new_message() cc_send.init('carControl') cc_send.valid = CS.canValid cc_send.carControl = CC carcontrol.send(cc_send.to_bytes()) return CC, events_bytes except Exception as e: print e
def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame, is_ldw_enabled): """Send actuators and hud commands to the car, send controlsstate and MPC logging""" CC = car.CarControl.new_message() CC.enabled = isEnabled(state) CC.actuators = actuators CC.cruiseControl.override = True CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled) # Some override values for Honda brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0)) # brake discount removes a sharp nonlinearity CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0) CC.cruiseControl.accelOverride = CI.calc_accel_override(CS.aEgo, sm['plan'].aTarget, CS.vEgo, sm['plan'].vTarget) CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS) CC.hudControl.speedVisible = isEnabled(state) CC.hudControl.lanesVisible = isEnabled(state) CC.hudControl.leadVisible = sm['plan'].hasLead right_lane_visible = sm['pathPlan'].rProb > 0.5 left_lane_visible = sm['pathPlan'].lProb > 0.5 CC.hudControl.rightLaneVisible = bool(right_lane_visible) CC.hudControl.leftLaneVisible = bool(left_lane_visible) recent_blinker = (sm.frame - last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown ldw_allowed = CS.vEgo > 31 * CV.MPH_TO_MS and not recent_blinker and is_ldw_enabled and not isActive(state) md = sm['model'] if len(md.meta.desirePrediction): l_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeLeft - 1] r_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeRight - 1] l_lane_close = left_lane_visible and (sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET)) r_lane_close = right_lane_visible and (sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET)) if ldw_allowed: CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close) CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close) if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart: AM.add(sm.frame, 'ldwPermanent', False) events.append(create_event('ldw', [ET.PERMANENT])) AM.process_alerts(sm.frame) CC.hudControl.visualAlert = AM.visual_alert if not read_only: # send car controls over can can_sends = CI.apply(CC) pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) force_decel = driver_status.awareness < 0. # controlsState dat = messaging.new_message() dat.init('controlsState') dat.valid = CS.canValid dat.controlsState = { "alertText1": AM.alert_text_1, "alertText2": AM.alert_text_2, "alertSize": AM.alert_size, "alertStatus": AM.alert_status, "alertBlinkingRate": AM.alert_rate, "alertType": AM.alert_type, "alertSound": AM.audible_alert, "awarenessStatus": max(driver_status.awareness, -0.1) if isEnabled(state) else 1.0, "driverMonitoringOn": bool(driver_status.face_detected), "canMonoTimes": list(CS.canMonoTimes), "planMonoTime": sm.logMonoTime['plan'], "pathPlanMonoTime": sm.logMonoTime['pathPlan'], "enabled": isEnabled(state), "active": isActive(state), "vEgo": CS.vEgo, "vEgoRaw": CS.vEgoRaw, "angleSteers": CS.steeringAngle, "curvature": VM.calc_curvature((CS.steeringAngle - sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD, CS.vEgo), "steerOverride": CS.steeringPressed, "state": state, "engageable": not bool(get_events(events, [ET.NO_ENTRY])), "longControlState": LoC.long_control_state, "vPid": float(LoC.v_pid), "vCruise": float(v_cruise_kph), "upAccelCmd": float(LoC.pid.p), "uiAccelCmd": float(LoC.pid.i), "ufAccelCmd": float(LoC.pid.f), "angleSteersDes": float(LaC.angle_steers_des), "vTargetLead": float(v_acc), "aTarget": float(a_acc), "jerkFactor": float(sm['plan'].jerkFactor), "gpsPlannerActive": sm['plan'].gpsPlannerActive, "vCurvature": sm['plan'].vCurvature, "decelForModel": sm['plan'].longitudinalPlanSource == log.Plan.LongitudinalPlanSource.model, "cumLagMs": -rk.remaining * 1000., "startMonoTime": int(start_time * 1e9), "mapValid": sm['plan'].mapValid, "forceDecel": bool(force_decel), } if CP.lateralTuning.which() == 'pid': dat.controlsState.lateralControlState.pidState = lac_log elif CP.lateralTuning.which() == 'lqr': dat.controlsState.lateralControlState.lqrState = lac_log elif CP.lateralTuning.which() == 'indi': dat.controlsState.lateralControlState.indiState = lac_log pm.send('controlsState', dat) # carState cs_send = messaging.new_message() cs_send.init('carState') cs_send.valid = CS.canValid cs_send.carState = CS cs_send.carState.events = events pm.send('carState', cs_send) # carEvents - logged every second or on change events_bytes = events_to_bytes(events) if (sm.frame % int(1. / DT_CTRL) == 0) or (events_bytes != events_prev): ce_send = messaging.new_message() ce_send.init('carEvents', len(events)) ce_send.carEvents = events pm.send('carEvents', ce_send) # carParams - logged every 50 seconds (> 1 per segment) if (sm.frame % int(50. / DT_CTRL) == 0): cp_send = messaging.new_message() cp_send.init('carParams') cp_send.carParams = CP pm.send('carParams', cp_send) # carControl cc_send = messaging.new_message() cc_send.init('carControl') cc_send.valid = CS.canValid cc_send.carControl = CC pm.send('carControl', cc_send) return CC, events_bytes
def update(self, c): # ******************* do can recv ******************* canMonoTimes = [] self.cp.update(int(sec_since_boot() * 1e9), False) self.CS.update(self.cp) # create message ret = car.CarState.new_message() #2018.09.05 add generic toggle for testing steering max# #ret.genericToggle = self.CS.generic_toggle # speeds ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw #2018.09.04 change to vehicle yaw rate values ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) ## ret.yawRate = self.CS.yaw_rate 2018.09.10 use yaw calculation from angle ret.standstill = self.CS.standstill ret.wheelSpeeds.fl = self.CS.v_wheel_fl ret.wheelSpeeds.fr = self.CS.v_wheel_fr ret.wheelSpeeds.rl = self.CS.v_wheel_rl ret.wheelSpeeds.rr = self.CS.v_wheel_rr # gas pedal #ret.gas = self.CS.car_gas / 256.0 ret.gas = self.CS.car_gas #2018.09.13 12:39AM change gas to car_gas without deviding #TODO 2018.09.05 check Throttle report operator override match this if not self.CP.enableGasInterceptor: ret.gasPressed = self.CS.pedal_gas > 0 else: ret.gasPressed = self.CS.user_gas_pressed # brake pedal ret.brake = self.CS.user_brake ret.brakePressed = self.CS.brake_pressed != 0 # FIXME: read sendcan for brakelights brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.SOUL else 0.1 #2018.09.03 change CIVIC to soul ret.brakeLights = bool(self.CS.brake_switch or c.actuators.brake > brakelights_threshold) # steering wheel ret.steeringAngle = self.CS.angle_steers # ret.steeringRate = self.CS.angle_steers_rate #2018.09.11 TODO some can duplication error? or not reading # gear shifter lever (#2018.09.04 define in carstate.py ret.gearShifter = self.CS.gear_shifter #2018.09.05 #TODO find steering torque value from steering wheel ret.steeringTorque = self.CS.steer_torque_driver ret.steeringPressed = self.CS.steer_override #2018.09.02 this come values py when steering operator override =1 #2018.09.02 TODO need to check is matter, we not using kia cruise control # cruise state #ret.cruiseState.enabled = self.CS.pcm_acc_status != 0 ret.cruiseState.enabled = False #2018.09.11 if use Openpilot cruise, no need, this should set to 0, need set to False #ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS #2018.09.11 check in controlsd.py if not enable cruise, this not needed ret.cruiseState.speed = 0 # 2018.09.11 check in controlsd.py if not enable cruise, this not needed, set to 0 ret.cruiseState.available = bool(self.CS.main_on) #2018.09.11 not use anywhere else beside interface.py ret.cruiseState.speedOffset = self.CS.cruise_speed_offset #2018.09.11 finally not use in controlsd.py if we use interceptor and enablecruise=false ret.cruiseState.standstill = False # TODO: button presses buttonEvents = [] ret.leftBlinker = bool(self.CS.left_blinker_on) ret.rightBlinker = bool(self.CS.right_blinker_on) ret.doorOpen = not self.CS.door_all_closed ret.seatbeltUnlatched = not self.CS.seatbelt if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: be = car.CarState.ButtonEvent.new_message() be.type = 'leftBlinker' be.pressed = self.CS.left_blinker_on != 0 buttonEvents.append(be) if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: be = car.CarState.ButtonEvent.new_message() be.type = 'rightBlinker' be.pressed = self.CS.right_blinker_on != 0 buttonEvents.append(be) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_buttons != 0: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: be.type = 'accelCruise' elif but == CruiseButtons.DECEL_SET: be.type = 'decelCruise' elif but == CruiseButtons.CANCEL: be.type = 'cancel' elif but == CruiseButtons.MAIN: be.type = 'altButton3' buttonEvents.append(be) if self.CS.cruise_setting != self.CS.prev_cruise_setting: be = car.CarState.ButtonEvent.new_message() be.type = 'unknown' if self.CS.cruise_setting != 0: be.pressed = True but = self.CS.cruise_setting else: be.pressed = False but = self.CS.prev_cruise_setting if but == 1: be.type = 'altButton1' # TODO: more buttons? buttonEvents.append(be) ret.buttonEvents = buttonEvents # events # TODO: I don't like the way capnp does enums # These strings aren't checked at compile time events = [] if not self.CS.can_valid: self.can_invalid_count += 1 if self.can_invalid_count >= 5: events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) else: self.can_invalid_count = 0 if self.CS.steer_error: events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) elif self.CS.steer_warning: events.append(create_event('steerTempUnavailable', [ET.WARNING])) if self.CS.brake_error: events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if not ret.gearShifter == 'drive': events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.doorOpen: events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.seatbeltUnlatched: events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CS.esp_disabled: events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.main_on: events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gearShifter == "reverse": events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) #remove brake hold and #electric parking brake if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) # disable on pedals rising edge or when brake is pressed and speed isn't zero if (ret.gasPressed and not self.gas_pressed_prev) or \ (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low if self.CP.enableCruise and not ret.cruiseState.enabled and c.actuators.brake <= 0.: # non loud alert if cruise disbales below 25mph as expected (+ a little margin) if ret.vEgo < self.CP.minEnableSpeed + 2.: events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) else: events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001: events.append(create_event('manualRestart', [ET.WARNING])) cur_time = sec_since_boot() enable_pressed = False # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in ["accelCruise", "decelCruise"] and not b.pressed: self.last_enable_pressed = cur_time enable_pressed = True # do disable on button down if b.type == "cancel" and b.pressed: events.append(create_event('buttonCancel', [ET.USER_DISABLE])) if self.CP.enableCruise: # KEEP THIS EVENT LAST! send enable event if button is pressed and there are # NO_ENTRY events, so controlsd will display alerts. Also not send enable events # too close in time, so a no_entry will not be followed by another one. # TODO: button press should be the only thing that triggers enble if ((cur_time - self.last_enable_pressed) < 0.2 and (cur_time - self.last_enable_sent) > 0.2 and ret.cruiseState.enabled) or \ (enable_pressed and get_events(events, [ET.NO_ENTRY])): events.append(create_event('buttonEnable', [ET.ENABLE])) self.last_enable_sent = cur_time elif enable_pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) ret.events = events ret.canMonoTimes = canMonoTimes # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed # cast to reader so it can't be modified return ret.as_reader()
def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM): """Compute conditional state transitions and execute actions on state transitions""" enabled = isEnabled(state) v_cruise_kph_last = v_cruise_kph # if stock cruise is completely disabled, then we can use our own set speed logic if not CP.enableCruise: v_cruise_kph = update_v_cruise(v_cruise_kph, CS.buttonEvents, enabled) elif CP.enableCruise and CS.cruiseState.enabled: v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH # decrease the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state soft_disable_timer = max(0, soft_disable_timer - 1) # DISABLED if state == State.disabled: if get_events(events, [ET.ENABLE]): if get_events(events, [ET.NO_ENTRY]): for e in get_events(events, [ET.NO_ENTRY]): AM.add(str(e) + "NoEntry", enabled) else: if get_events(events, [ET.PRE_ENABLE]): state = State.preEnabled else: state = State.enabled AM.add("enable", enabled) v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, v_cruise_kph_last) # ENABLED elif state == State.enabled: if get_events(events, [ET.USER_DISABLE]): state = State.disabled AM.add("disable", enabled) elif get_events(events, [ET.IMMEDIATE_DISABLE]): state = State.disabled for e in get_events(events, [ET.IMMEDIATE_DISABLE]): AM.add(e, enabled) elif get_events(events, [ET.SOFT_DISABLE]): state = State.softDisabling soft_disable_timer = 300 # 3s for e in get_events(events, [ET.SOFT_DISABLE]): AM.add(e, enabled) # SOFT DISABLING elif state == State.softDisabling: if get_events(events, [ET.USER_DISABLE]): state = State.disabled AM.add("disable", enabled) elif get_events(events, [ET.IMMEDIATE_DISABLE]): state = State.disabled for e in get_events(events, [ET.IMMEDIATE_DISABLE]): AM.add(e, enabled) elif not get_events(events, [ET.SOFT_DISABLE]): # no more soft disabling condition, so go back to ENABLED state = State.enabled elif get_events(events, [ET.SOFT_DISABLE]) and soft_disable_timer > 0: for e in get_events(events, [ET.SOFT_DISABLE]): AM.add(e, enabled) elif soft_disable_timer <= 0: state = State.disabled # PRE ENABLING elif state == State.preEnabled: if get_events(events, [ET.USER_DISABLE]): state = State.disabled AM.add("disable", enabled) elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]): state = State.disabled for e in get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]): AM.add(e, enabled) elif not get_events(events, [ET.PRE_ENABLE]): state = State.enabled return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last
def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, saturated_count, dragon_lat_control, dragon_display_steering_limit_alert, dragon_lead_car_moving_alert): """Given the state, this function returns an actuators packet""" actuators = car.CarControl.Actuators.new_message() enabled = isEnabled(state) active = isActive(state) if CS.leftBlinker or CS.rightBlinker: last_blinker_frame = frame if plan.fcw: # send FCW alert if triggered by planner AM.add(frame, "fcw", enabled) elif CS.stockFcw: # send a silent alert when stock fcw triggers, since the car is already beeping AM.add(frame, "fcwStock", enabled) # State specific actions if state in [State.preEnabled, State.disabled]: if dragon_lead_car_moving_alert: for e in get_events(events, [ET.WARNING]): extra_text = "" if e in ["leadCarDetected", "leadCarMoving"]: AM.add(frame, e, enabled, extra_text_2=extra_text) LaC.reset() LoC.reset(v_pid=CS.vEgo) elif state in [State.enabled, State.softDisabling]: # parse warnings from car specific interface for e in get_events(events, [ET.WARNING]): extra_text = "" if e == "belowSteerSpeed": if is_metric: extra_text = str( int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph" else: extra_text = str( int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph" AM.add(frame, e, enabled, extra_text_2=extra_text) plan_age = DT_CTRL * (frame - rcv_frame['plan']) dt = min( plan_age, LON_MPC_STEP + DT_CTRL ) + DT_CTRL # no greater than dt mpc + dt, to prevent too high extraps a_acc_sol = plan.aStart + (dt / LON_MPC_STEP) * (plan.aTarget - plan.aStart) v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0 # Gas/Brake PID loop actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.gasPressed, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP) # Steering PID loop and lateral MPC actuators.steer, actuators.steerAngle, lac_log = LaC.update( active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CS.steeringRateLimited, CP, path_plan) # Check for difference between desired angle and angle for angle based control angle_control_saturated = CP.steerControlType == car.CarParams.SteerControlType.angle and \ abs(actuators.steerAngle - CS.steeringAngle) > STEER_ANGLE_SATURATION_THRESHOLD saturated_count = saturated_count + 1 if angle_control_saturated and not CS.steeringPressed and active else 0 if dragon_display_steering_limit_alert and dragon_lat_control: # Send a "steering required alert" if saturation count has reached the limit if (lac_log.saturated and not CS.steeringPressed) or ( saturated_count > STEER_ANGLE_SATURATION_TIMEOUT): # Check if we deviated from the path left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.1 right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.1 if left_deviation or right_deviation: AM.add(frame, "steerSaturated", enabled) # Parse permanent warnings to display constantly for e in get_events(events, [ET.PERMANENT]): extra_text_1, extra_text_2 = "", "" if e == "calibrationIncomplete": extra_text_1 = str(cal_perc) + "%" if is_metric: extra_text_2 = str(int(round( Filter.MIN_SPEED * CV.MS_TO_KPH))) + " kph" else: extra_text_2 = str(int(round( Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph" AM.add(frame, str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2) return actuators, v_cruise_kph, v_acc_sol, a_acc_sol, lac_log, last_blinker_frame, saturated_count
def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, live100, AM, driver_status, LaC, LoC, angle_model_bias, passive, start_time, params, v_acc, a_acc): """Send actuators and hud commands to the car, send live100 and MPC logging""" plan_ts = plan.logMonoTime plan = plan.plan CC = car.CarControl.new_message() if not passive: CC.enabled = isEnabled(state) CC.actuators = actuators CC.cruiseControl.override = True CC.cruiseControl.cancel = not CP.enableCruise or ( not isEnabled(state) and CS.cruiseState.enabled) # Some override values for Honda brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0) ) # brake discount removes a sharp nonlinearity CC.cruiseControl.speedOverride = float( max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0) CC.cruiseControl.accelOverride = CI.calc_accel_override( CS.aEgo, plan.aTarget, CS.vEgo, plan.vTarget) CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS) CC.hudControl.speedVisible = isEnabled(state) CC.hudControl.lanesVisible = isEnabled(state) CC.hudControl.leadVisible = plan.hasLead CC.hudControl.rightLaneVisible = bool(path_plan.pathPlan.rProb > 0.5) CC.hudControl.leftLaneVisible = bool(path_plan.pathPlan.lProb > 0.5) CC.hudControl.visualAlert = AM.visual_alert CC.hudControl.audibleAlert = AM.audible_alert # send car controls over can CI.apply(CC) force_decel = driver_status.awareness < 0. # live100 dat = messaging.new_message() dat.init('live100') dat.live100 = { "alertText1": AM.alert_text_1, "alertText2": AM.alert_text_2, "alertSize": AM.alert_size, "alertStatus": AM.alert_status, "alertBlinkingRate": AM.alert_rate, "alertType": AM.alert_type, "alertSound": "", # no EON sounds yet "awarenessStatus": max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0, "driverMonitoringOn": bool(driver_status.monitor_on), "canMonoTimes": list(CS.canMonoTimes), "planMonoTime": plan_ts, "pathPlanMonoTime": path_plan.logMonoTime, "enabled": isEnabled(state), "active": isActive(state), "vEgo": CS.vEgo, "vEgoRaw": CS.vEgoRaw, "angleSteers": CS.steeringAngle, "curvature": VM.calc_curvature(CS.steeringAngle * CV.DEG_TO_RAD, CS.vEgo), "steerOverride": CS.steeringPressed, "state": state, "engageable": not bool(get_events(events, [ET.NO_ENTRY])), "longControlState": LoC.long_control_state, "vPid": float(LoC.v_pid), "vCruise": float(v_cruise_kph), "upAccelCmd": float(LoC.pid.p), "uiAccelCmd": float(LoC.pid.i), "ufAccelCmd": float(LoC.pid.f), "angleSteersDes": float(LaC.angle_steers_des), "upSteer": float(LaC.pid.p), "uiSteer": float(LaC.pid.i), "ufSteer": float(LaC.pid.f), "vTargetLead": float(v_acc), "aTarget": float(a_acc), "jerkFactor": float(plan.jerkFactor), "angleModelBias": float(angle_model_bias), "gpsPlannerActive": plan.gpsPlannerActive, "vCurvature": plan.vCurvature, "decelForTurn": plan.decelForTurn, "cumLagMs": -rk.remaining * 1000., "startMonoTime": start_time, "mapValid": plan.mapValid, "forceDecel": bool(force_decel), } live100.send(dat.to_bytes()) # carState cs_send = messaging.new_message() cs_send.init('carState') cs_send.carState = CS cs_send.carState.events = events carstate.send(cs_send.to_bytes()) # carControl cc_send = messaging.new_message() cc_send.init('carControl') cc_send.carControl = CC carcontrol.send(cc_send.to_bytes()) if (rk.frame % 36000) == 0: # update angle offset every 6 minutes params.put("ControlsParams", json.dumps({'angle_model_bias': angle_model_bias})) return CC
def update(self, c, can_strings): # ******************* do can recv ******************* self.cp.update_strings(can_strings) self.cp_cam.update_strings(can_strings) self.CS.update(self.cp, self.cp_cam) # create message ret = car.CarState.new_message() ret.canValid = self.cp.can_valid and self.cp_cam.can_valid # Bosch Lane Polynomials ret.lPoly.c3 = self.CS.l_poly[3] ret.lPoly.c2 = self.CS.l_poly[2] ret.lPoly.c1 = self.CS.l_poly[1] ret.lPoly.c0 = self.CS.l_poly[0] ret.lPoly.prob = self.CS.l_prob ret.lPoly.distVis = self.CS.l_distVis ret.lPoly.isSolid = bool(self.CS.l_isSolid) ret.lPoly.isDashed = bool(self.CS.l_isDashed) ret.rPoly.c3 = self.CS.r_poly[3] ret.rPoly.c2 = self.CS.r_poly[2] ret.rPoly.c1 = self.CS.r_poly[1] ret.rPoly.c0 = self.CS.r_poly[0] ret.rPoly.prob = self.CS.r_prob ret.rPoly.distVis = self.CS.r_distVis ret.rPoly.isSolid = bool(self.CS.r_isSolid) ret.rPoly.isDashed = bool(self.CS.r_isDashed) ret.lAdjPoly.c3 = self.CS.lAdj_poly[3] ret.lAdjPoly.c2 = self.CS.lAdj_poly[2] ret.lAdjPoly.c1 = self.CS.lAdj_poly[1] ret.lAdjPoly.c0 = self.CS.lAdj_poly[0] ret.lAdjPoly.prob = self.CS.lAdj_prob ret.lAdjPoly.distVis = self.CS.lAdj_distVis ret.lAdjPoly.isSolid = bool(self.CS.lAdj_isSolid) ret.lAdjPoly.isDashed = bool(self.CS.lAdj_isDashed) ret.rAdjPoly.c3 = self.CS.rAdj_poly[3] ret.rAdjPoly.c2 = self.CS.rAdj_poly[2] ret.rAdjPoly.c1 = self.CS.rAdj_poly[1] ret.rAdjPoly.c0 = self.CS.rAdj_poly[0] ret.rAdjPoly.prob = self.CS.rAdj_prob ret.rAdjPoly.distVis = self.CS.rAdj_distVis ret.rAdjPoly.isSolid = bool(self.CS.rAdj_isSolid) ret.rAdjPoly.isDashed = bool(self.CS.rAdj_isDashed) # speeds ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) ret.standstill = self.CS.standstill ret.wheelSpeeds.fl = self.CS.v_wheel_fl ret.wheelSpeeds.fr = self.CS.v_wheel_fr ret.wheelSpeeds.rl = self.CS.v_wheel_rl ret.wheelSpeeds.rr = self.CS.v_wheel_rr # gas pedal ret.gas = self.CS.car_gas / 256.0 if not self.CP.enableGasInterceptor: ret.gasPressed = self.CS.pedal_gas > 0 else: ret.gasPressed = self.CS.user_gas_pressed # brake pedal ret.brake = self.CS.user_brake ret.brakePressed = self.CS.brake_pressed != 0 # FIXME: read sendcan for brakelights brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1 ret.brakeLights = bool(self.CS.brake_switch or c.actuators.brake > brakelights_threshold) # steering wheel ret.steeringAngle = self.CS.angle_steers ret.steeringRate = self.CS.angle_steers_rate # gear shifter lever ret.gearShifter = self.CS.gear_shifter ret.steeringTorque = self.CS.steer_torque_driver ret.steeringTorqueEps = self.CS.steer_torque_motor ret.steeringPressed = self.CS.steer_override # cruise state ret.cruiseState.enabled = self.CS.pcm_acc_status != 0 ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.available = bool( self.CS.main_on) and not bool(self.CS.cruise_mode) ret.cruiseState.speedOffset = self.CS.cruise_speed_offset ret.cruiseState.standstill = False # TODO: button presses buttonEvents = [] ret.leftBlinker = bool(self.CS.left_blinker_on) ret.rightBlinker = bool(self.CS.right_blinker_on) ret.doorOpen = not self.CS.door_all_closed ret.seatbeltUnlatched = not self.CS.seatbelt ret.stockAeb = self.CS.stock_aeb ret.stockFcw = self.CS.stock_fcw if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: be = car.CarState.ButtonEvent.new_message() be.type = ButtonType.leftBlinker be.pressed = self.CS.left_blinker_on != 0 buttonEvents.append(be) if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: be = car.CarState.ButtonEvent.new_message() be.type = ButtonType.rightBlinker be.pressed = self.CS.right_blinker_on != 0 buttonEvents.append(be) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() be.type = ButtonType.unknown if self.CS.cruise_buttons != 0: be.pressed = True but = self.CS.cruise_buttons else: be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: be.type = ButtonType.accelCruise elif but == CruiseButtons.DECEL_SET: be.type = ButtonType.decelCruise elif but == CruiseButtons.CANCEL: be.type = ButtonType.cancel elif but == CruiseButtons.MAIN: be.type = ButtonType.altButton3 buttonEvents.append(be) if self.CS.cruise_setting != self.CS.prev_cruise_setting: be = car.CarState.ButtonEvent.new_message() be.type = ButtonType.unknown if self.CS.cruise_setting != 0: be.pressed = True but = self.CS.cruise_setting else: be.pressed = False but = self.CS.prev_cruise_setting if but == 1: be.type = ButtonType.altButton1 # TODO: more buttons? buttonEvents.append(be) ret.buttonEvents = buttonEvents # events events = [] if self.CS.steer_error: events.append( create_event( 'steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) elif self.CS.steer_warning: events.append(create_event('steerTempUnavailable', [ET.WARNING])) if self.CS.brake_error: events.append( create_event( 'brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if not ret.gearShifter == GearShifter.drive: events.append( create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.doorOpen: events.append( create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.seatbeltUnlatched: events.append( create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CS.esp_disabled: events.append( create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.main_on or self.CS.cruise_mode: events.append( create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gearShifter == GearShifter.reverse: events.append( create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CS.brake_hold and self.CS.CP.carFingerprint not in HONDA_BOSCH: events.append( create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CS.park_brake: events.append( create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) # disable on pedals rising edge or when brake is pressed and speed isn't zero if (ret.gasPressed and not self.gas_pressed_prev) or \ (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): events.append( create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low if self.CP.enableCruise and not ret.cruiseState.enabled and ( c.actuators.brake <= 0. or not self.CP.openpilotLongitudinalControl): # non loud alert if cruise disbales below 25mph as expected (+ a little margin) if ret.vEgo < self.CP.minEnableSpeed + 2.: events.append( create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) else: events.append( create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001: events.append(create_event('manualRestart', [ET.WARNING])) cur_time = self.frame * DT_CTRL enable_pressed = False # handle button presses for b in ret.buttonEvents: # do enable on both accel and decel buttons if b.type in [ButtonType.accelCruise, ButtonType.decelCruise ] and not b.pressed: self.last_enable_pressed = cur_time enable_pressed = True # do disable on button down if b.type == "cancel" and b.pressed: events.append(create_event('buttonCancel', [ET.USER_DISABLE])) if self.CP.enableCruise: # KEEP THIS EVENT LAST! send enable event if button is pressed and there are # NO_ENTRY events, so controlsd will display alerts. Also not send enable events # too close in time, so a no_entry will not be followed by another one. # TODO: button press should be the only thing that triggers enable if ((cur_time - self.last_enable_pressed) < 0.2 and (cur_time - self.last_enable_sent) > 0.2 and ret.cruiseState.enabled) or \ (enable_pressed and get_events(events, [ET.NO_ENTRY])): events.append(create_event('buttonEnable', [ET.ENABLE])) self.last_enable_sent = cur_time elif enable_pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) ret.events = events # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed # cast to reader so it can't be modified return ret.as_reader()