Ejemplo n.º 1
0
    def update(self, enabled, CS, frame, actuators, hud_v_cruise,
               hud_show_lanes, hud_show_car, hud_alert, paddleFrame):

        ##print("paddleFrame %d", paddleFrame)

        P = self.params
        # Send CAN commands.
        can_sends = []

        # STEER
        if (frame % P.STEER_STEP) == 0:
            #lkas_enabled = enabled and not CS.out.steerWarning and CS.lkMode and CS.out.vEgo > P.MIN_STEER_SPEED
            lkas_enabled = enabled and not CS.out.steerWarning and CS.out.vEgo > P.MIN_STEER_SPEED
            if lkas_enabled:
                new_steer = actuators.steer * P.STEER_MAX
                apply_steer = apply_std_steer_torque_limits(
                    new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
                self.steer_rate_limited = new_steer != apply_steer
            else:
                apply_steer = 0

            self.apply_steer_last = apply_steer
            idx = (frame // P.STEER_STEP) % 4

            can_sends.append(
                gmcan.create_steering_control(self.packer_pt,
                                              CanBus.POWERTRAIN, apply_steer,
                                              idx, lkas_enabled))

        # GAS/BRAKE
        # no output if not enabled, but keep sending keepalive messages
        # treat pedals as one
        final_pedal = actuators.gas - actuators.brake

        if not enabled:
            # Stock ECU sends max regen when not enabled.
            apply_gas = P.MAX_ACC_REGEN
            apply_brake = 0
        else:
            apply_gas = int(
                round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
            apply_brake = int(
                round(interp(final_pedal, P.BRAKE_LOOKUP_BP,
                             P.BRAKE_LOOKUP_V)))

        # Gas/regen and brakes - all at 25Hz
        if (frame % 4) == 0:
            idx = (frame // 4) % 4

            ## 인게이지 상태 아니고 오토홀드 스위치 들어가 있고 엑셀 페달 밟는 상태가 아니고 기어가 d,l에 있으며 속도가 1.8kph 이하일때 동작
            ##if not enabled and CS.autoHold and not CS.out.gasPressed and CS.out.gearShifter == 'drive' and CS.out.vEgo < 0.01 and not CS.regenPaddlePressed:
            if not enabled and CS.autoHold and CS.autoHoldActive and not CS.out.gasPressed and CS.out.gearShifter == 'drive' and CS.out.vEgo < 0.01 and not CS.regenPaddlePressed:
                car_stopping = apply_gas < P.ZERO_GAS

                standstill = CS.out.vEgo < 0.01  ## neokii test
                at_full_stop = standstill and car_stopping

                near_stop = (CS.out.vEgo <
                             P.NEAR_STOP_BRAKE_PHASE) and car_stopping
                ##if at_full_stop :
                ##print("A------------------")
                ##print("apply_brake", apply_brake)
                ##print("car_stopping", car_stopping)
                ##print("apply_gas ", apply_gas)
                ##print("P.ZERO_GAS ", P.ZERO_GAS)
                ##print("standstill ", standstill)
                ##print("CS.pcm_acc_status ", CS.pcm_acc_status)
                ##print("AccState.STANDSTILL ", AccState.STANDSTILL)
                ##print("at_full_stop", at_full_stop)
                ##print("near_stop ", near_stop)
                ##print("CS.out.vEgo ", CS.out.vEgo)
                ##print(" P.NEAR_STOP_BRAKE_PHASE ",  P.NEAR_STOP_BRAKE_PHASE)

                ##print("enabled ", enabled)
                #print("CS.autoHold ", CS.autoHold)
                #print("CS.autoHoldActive ", CS.autoHoldActive)
                ##print("CS.gas", CS.out.gas)
                ##print("CS.gasPressed", CS.out.gasPressed)
                ##print("gearShifter ", CS.out.gearShifter)
                ####at_full_stop = standstill and car_stopping
                ####can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, apply_brake, idx, near_stop, at_full_stop))
                can_sends.append(
                    gmcan.create_friction_brake_command(
                        self.packer_ch, CanBus.CHASSIS, apply_brake, idx,
                        near_stop, at_full_stop))
                CS.autoHoldActivated = True

            else:

                car_stopping = apply_gas < P.ZERO_GAS
                standstill = CS.pcm_acc_status == AccState.STANDSTILL

                at_full_stop = enabled and standstill and car_stopping
                near_stop = enabled and (
                    CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) and car_stopping

                ##print("B------------------")
                ##print("apply_brake", apply_brake)
                ##print("car_stopping", car_stopping)
                ##print("apply_gas ", apply_gas)
                ##print("P.ZERO_GAS ", P.ZERO_GAS)
                ##print("standstill ", standstill)
                ##print("CS.pcm_acc_status ", CS.pcm_acc_status)
                ##print("AccState.STANDSTILL ", AccState.STANDSTILL)
                ##print("at_full_stop", at_full_stop)
                ##print("near_stop ", near_stop)
                ##print("CS.out.vEgo ", CS.out.vEgo)
                ##print(" P.NEAR_STOP_BRAKE_PHASE ",  P.NEAR_STOP_BRAKE_PHASE)

                ##print("enabled ", enabled)
                #print("CS.autoHold ", CS.autoHold)
                #print("CS.autoHoldActive ", CS.autoHoldActive)
                ##print("CS.gas", CS.out.gas)
                ##print("CS.gasPressed", CS.out.gasPressed)
                ##print("gearShifter ", CS.out.gearShifter)
                can_sends.append(
                    gmcan.create_friction_brake_command(
                        self.packer_ch, CanBus.CHASSIS, apply_brake, idx,
                        near_stop, at_full_stop))
                CS.autoHoldActivated = False

                # Auto-resume from full stop by resetting ACC control
                acc_enabled = enabled

                if standstill and not car_stopping:
                    acc_enabled = False

                can_sends.append(
                    gmcan.create_gas_regen_command(self.packer_pt,
                                                   CanBus.POWERTRAIN,
                                                   apply_gas, idx, acc_enabled,
                                                   at_full_stop))

        follow_level = CS.get_follow_level()

        # Send dashboard UI commands (ACC status), 25hz
        if (frame % 4) == 0:
            send_fcw = hud_alert == VisualAlert.fcw
            can_sends.append(
                gmcan.create_acc_dashboard_command(self.packer_pt,
                                                   CanBus.POWERTRAIN, enabled,
                                                   hud_v_cruise * CV.MS_TO_KPH,
                                                   hud_show_car, send_fcw,
                                                   follow_level))

        # Radar needs to know current speed and yaw rate (50hz),
        # and that ADAS is alive (10hz)
        time_and_headlights_step = 10
        tt = frame * DT_CTRL

        if frame % time_and_headlights_step == 0:
            idx = (frame // time_and_headlights_step) % 4
            can_sends.append(
                gmcan.create_adas_time_status(CanBus.OBSTACLE,
                                              int((tt - self.start_time) * 60),
                                              idx))
            can_sends.append(
                gmcan.create_adas_headlights_status(self.packer_obj,
                                                    CanBus.OBSTACLE))

        speed_and_accelerometer_step = 2
        if frame % speed_and_accelerometer_step == 0:
            idx = (frame // speed_and_accelerometer_step) % 4
            can_sends.append(
                gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
            can_sends.append(
                gmcan.create_adas_accelerometer_speed_status(
                    CanBus.OBSTACLE, CS.out.vEgo, idx))

        if frame % P.ADAS_KEEPALIVE_STEP == 0:
            can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)

        # Show green icon when LKA torque is applied, and
        # alarming orange icon when approaching torque limit.
        # If not sent again, LKA icon disappears in about 5 seconds.
        # Conveniently, sending camera message periodically also works as a keepalive.
        lka_active = CS.lkas_status == 1
        lka_critical = lka_active and abs(actuators.steer) > 0.9
        lka_icon_status = (lka_active, lka_critical)
        if frame % P.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last:
            steer_alert = hud_alert == VisualAlert.steerRequired
            can_sends.append(
                gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active,
                                              lka_critical, steer_alert))
            self.lka_icon_status_last = lka_icon_status

        return can_sends
Ejemplo n.º 2
0
    def update(self, enabled, CS, frame, actuators, hud_v_cruise,
               hud_show_lanes, hud_show_car, hud_alert):

        P = self.params

        # Send CAN commands.
        can_sends = []

        # STEER
        if (frame % P.STEER_STEP) == 0:
            lkas_enabled = enabled and not CS.out.steerWarning and CS.out.vEgo > P.MIN_STEER_SPEED
            if lkas_enabled:
                new_steer = actuators.steer * P.STEER_MAX
                apply_steer = apply_std_steer_torque_limits(
                    new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
                self.steer_rate_limited = new_steer != apply_steer
            else:
                apply_steer = 0

            self.apply_steer_last = apply_steer
            idx = (frame // P.STEER_STEP) % 4

            can_sends.append(
                gmcan.create_steering_control(self.packer_pt,
                                              CanBus.POWERTRAIN, apply_steer,
                                              idx, lkas_enabled))

        # GAS/BRAKE
        # no output if not enabled, but keep sending keepalive messages
        # treat pedals as one
        final_pedal = actuators.gas - actuators.brake

        if not enabled:
            # Stock ECU sends max regen when not enabled.
            apply_gas = P.MAX_ACC_REGEN
            apply_brake = 0
        else:
            apply_gas = int(
                round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
            apply_brake = int(
                round(interp(final_pedal, P.BRAKE_LOOKUP_BP,
                             P.BRAKE_LOOKUP_V)))

        # Gas/regen and brakes - all at 25Hz
        if (frame % 4) == 0:
            idx = (frame // 4) % 4

            at_full_stop = enabled and CS.out.standstill
            near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
            can_sends.append(
                gmcan.create_friction_brake_command(self.packer_ch,
                                                    CanBus.CHASSIS,
                                                    apply_brake, idx,
                                                    near_stop, at_full_stop))

            at_full_stop = enabled and CS.out.standstill

            can_sends.append(
                gmcan.create_gas_regen_command(self.packer_pt,
                                               CanBus.POWERTRAIN, apply_gas,
                                               idx, enabled, at_full_stop))

        # Send dashboard UI commands (ACC status), 25hz
        if (frame % 4) == 0:
            send_fcw = hud_alert == VisualAlert.fcw
            can_sends.append(
                gmcan.create_acc_dashboard_command(self.packer_pt,
                                                   CanBus.POWERTRAIN, enabled,
                                                   hud_v_cruise * CV.MS_TO_KPH,
                                                   hud_show_car, send_fcw))

        # Radar needs to know current speed and yaw rate (50hz),
        # and that ADAS is alive (10hz)
        time_and_headlights_step = 10
        tt = frame * DT_CTRL

        if frame % time_and_headlights_step == 0:
            idx = (frame // time_and_headlights_step) % 4
            can_sends.append(
                gmcan.create_adas_time_status(CanBus.OBSTACLE,
                                              int((tt - self.start_time) * 60),
                                              idx))
            can_sends.append(
                gmcan.create_adas_headlights_status(self.packer_obj,
                                                    CanBus.OBSTACLE))

        speed_and_accelerometer_step = 2
        if frame % speed_and_accelerometer_step == 0:
            idx = (frame // speed_and_accelerometer_step) % 4
            can_sends.append(
                gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
            can_sends.append(
                gmcan.create_adas_accelerometer_speed_status(
                    CanBus.OBSTACLE, CS.out.vEgo, idx))

        if frame % P.ADAS_KEEPALIVE_STEP == 0:
            can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)

        # Show green icon when LKA torque is applied, and
        # alarming orange icon when approaching torque limit.
        # If not sent again, LKA icon disappears in about 5 seconds.
        # Conveniently, sending camera message periodically also works as a keepalive.
        lka_active = CS.lkas_status == 1
        lka_critical = lka_active and abs(actuators.steer) > 0.9
        lka_icon_status = (lka_active, lka_critical)

        if frame % P.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last:
            steer_alert = hud_alert == VisualAlert.steerRequired
            can_sends.append(
                gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active,
                                              lka_critical, steer_alert))
            self.lka_icon_status_last = lka_icon_status

        return can_sends
Ejemplo n.º 3
0
    def update(self, CC, CS):
        actuators = CC.actuators
        hud_control = CC.hudControl
        hud_alert = hud_control.visualAlert
        hud_v_cruise = hud_control.setSpeed
        if hud_v_cruise > 70:
            hud_v_cruise = 0

        # Send CAN commands.
        can_sends = []

        # Steering (50Hz)
        # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just received the
        # next Panda loopback confirmation in the current CS frame.
        if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last:
            self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter
        elif (self.frame % self.params.STEER_STEP) == 0:
            lkas_enabled = CC.latActive and CS.out.vEgo > self.params.MIN_STEER_SPEED
            if lkas_enabled:
                new_steer = int(round(actuators.steer * self.params.STEER_MAX))
                apply_steer = apply_std_steer_torque_limits(
                    new_steer, self.apply_steer_last, CS.out.steeringTorque,
                    self.params)
                self.steer_rate_limited = new_steer != apply_steer
            else:
                apply_steer = 0

            self.apply_steer_last = apply_steer
            # GM EPS faults on any gap in received message counters. To handle transient OP/Panda safety sync issues at the
            # moment of disengaging, increment the counter based on the last message known to pass Panda safety checks.
            idx = (CS.lka_steering_cmd_counter + 1) % 4

            can_sends.append(
                gmcan.create_steering_control(self.packer_pt,
                                              CanBus.POWERTRAIN, apply_steer,
                                              idx, lkas_enabled))

        # Gas/regen and brakes - all at 25Hz
        if (self.frame % 4) == 0:
            if not CC.longActive:
                # Stock ECU sends max regen when not enabled
                self.apply_gas = self.params.MAX_ACC_REGEN
                self.apply_brake = 0
            else:
                self.apply_gas = int(
                    round(
                        interp(actuators.accel, self.params.GAS_LOOKUP_BP,
                               self.params.GAS_LOOKUP_V)))
                self.apply_brake = int(
                    round(
                        interp(actuators.accel, self.params.BRAKE_LOOKUP_BP,
                               self.params.BRAKE_LOOKUP_V)))

            idx = (self.frame // 4) % 4

            at_full_stop = CC.longActive and CS.out.standstill
            near_stop = CC.longActive and (CS.out.vEgo <
                                           self.params.NEAR_STOP_BRAKE_PHASE)
            # GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation
            can_sends.append(
                gmcan.create_gas_regen_command(self.packer_pt,
                                               CanBus.POWERTRAIN,
                                               self.apply_gas, idx, CC.enabled,
                                               at_full_stop))
            can_sends.append(
                gmcan.create_friction_brake_command(self.packer_ch,
                                                    CanBus.CHASSIS,
                                                    self.apply_brake, idx,
                                                    near_stop, at_full_stop))

        # Send dashboard UI commands (ACC status), 25hz
        if (self.frame % 4) == 0:
            send_fcw = hud_alert == VisualAlert.fcw
            can_sends.append(
                gmcan.create_acc_dashboard_command(self.packer_pt,
                                                   CanBus.POWERTRAIN,
                                                   CC.enabled,
                                                   hud_v_cruise * CV.MS_TO_KPH,
                                                   hud_control.leadVisible,
                                                   send_fcw))

        # Radar needs to know current speed and yaw rate (50hz),
        # and that ADAS is alive (10hz)
        time_and_headlights_step = 10
        tt = self.frame * DT_CTRL

        if self.frame % time_and_headlights_step == 0:
            idx = (self.frame // time_and_headlights_step) % 4
            can_sends.append(
                gmcan.create_adas_time_status(CanBus.OBSTACLE,
                                              int((tt - self.start_time) * 60),
                                              idx))
            can_sends.append(
                gmcan.create_adas_headlights_status(self.packer_obj,
                                                    CanBus.OBSTACLE))

        speed_and_accelerometer_step = 2
        if self.frame % speed_and_accelerometer_step == 0:
            idx = (self.frame // speed_and_accelerometer_step) % 4
            can_sends.append(
                gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
            can_sends.append(
                gmcan.create_adas_accelerometer_speed_status(
                    CanBus.OBSTACLE, CS.out.vEgo, idx))

        if self.frame % self.params.ADAS_KEEPALIVE_STEP == 0:
            can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)

        # Show green icon when LKA torque is applied, and
        # alarming orange icon when approaching torque limit.
        # If not sent again, LKA icon disappears in about 5 seconds.
        # Conveniently, sending camera message periodically also works as a keepalive.
        lka_active = CS.lkas_status == 1
        lka_critical = lka_active and abs(actuators.steer) > 0.9
        lka_icon_status = (lka_active, lka_critical)
        if self.frame % self.params.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last:
            steer_alert = hud_alert in (VisualAlert.steerRequired,
                                        VisualAlert.ldw)
            can_sends.append(
                gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active,
                                              lka_critical, steer_alert))
            self.lka_icon_status_last = lka_icon_status

        new_actuators = actuators.copy()
        new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX
        new_actuators.gas = self.apply_gas
        new_actuators.brake = self.apply_brake

        self.frame += 1
        return new_actuators, can_sends
Ejemplo n.º 4
0
    def update(self, sendcan, enabled, CS, frame, actuators, \
               hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt):
        """ Controls thread """
        #update custom UI buttons and alerts
        CS.UE.update_custom_ui()
        if (frame % 1000 == 0):
            CS.cstm_btns.send_button_info()
            CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder, CS.cstm_btns.car_name)

        # Sanity check.
        if not self.allow_controls:
            return

        P = self.params
        # Send CAN commands.
        can_sends = []
        canbus = self.canbus

        ### STEER ###

        if (frame % P.STEER_STEP) == 0:
            lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > 3.
            if lkas_enabled:
                apply_steer = actuators.steer * P.STEER_MAX
                apply_steer = apply_std_steer_torque_limits(
                    apply_steer, self.apply_steer_last, CS.steer_torque_driver,
                    P)
            else:
                apply_steer = 0

            self.apply_steer_last = apply_steer
            idx = (frame / P.STEER_STEP) % 4
            if CS.cstm_btns.get_button_status("lka") == 0:
                apply_steer = 0
            if self.car_fingerprint == CAR.VOLT:
                can_sends.append(
                    gmcan.create_steering_control(self.packer_pt,
                                                  canbus.powertrain,
                                                  apply_steer, idx,
                                                  lkas_enabled))
            if self.car_fingerprint == CAR.CADILLAC_CT6:
                can_sends += gmcan.create_steering_control_ct6(
                    self.packer_pt, canbus, apply_steer, CS.v_ego, idx,
                    lkas_enabled)

        ### GAS/BRAKE ###

        if self.car_fingerprint == CAR.VOLT:
            # no output if not enabled, but keep sending keepalive messages
            # treat pedals as one
            final_pedal = actuators.gas - actuators.brake

            # *** apply pedal hysteresis ***
            final_brake, self.brake_steady = actuator_hystereses(
                final_pedal, self.pedal_steady)

            if not enabled:
                # Stock ECU sends max regen when not enabled.
                apply_gas = P.MAX_ACC_REGEN
                apply_brake = 0
            else:
                apply_gas = int(
                    round(interp(final_pedal, P.GAS_LOOKUP_BP,
                                 P.GAS_LOOKUP_V)))
                apply_brake = int(
                    round(
                        interp(final_pedal, P.BRAKE_LOOKUP_BP,
                               P.BRAKE_LOOKUP_V)))

            # Gas/regen and brakes - all at 25Hz
            if (frame % 4) == 0:
                idx = (frame / 4) % 4

                car_stopping = apply_gas < P.ZERO_GAS
                standstill = CS.pcm_acc_status == AccState.STANDSTILL
                at_full_stop = enabled and standstill and car_stopping
                near_stop = enabled and (
                    CS.v_ego < P.NEAR_STOP_BRAKE_PHASE) and car_stopping
                can_sends.append(
                    gmcan.create_friction_brake_command(
                        self.packer_ch, canbus.chassis, apply_brake, idx,
                        near_stop, at_full_stop))

                # Auto-resume from full stop by resetting ACC control
                acc_enabled = enabled
                if standstill and not car_stopping:
                    acc_enabled = False

                can_sends.append(
                    gmcan.create_gas_regen_command(self.packer_pt,
                                                   canbus.powertrain,
                                                   apply_gas, idx, acc_enabled,
                                                   at_full_stop))

            # Send dashboard UI commands (ACC status), 25hz
            if (frame % 4) == 0:
                can_sends.append(
                    gmcan.create_acc_dashboard_command(
                        self.packer_pt, canbus.powertrain, enabled,
                        hud_v_cruise * CV.MS_TO_KPH, hud_show_car))

            # Radar needs to know current speed and yaw rate (50hz),
            # and that ADAS is alive (10hz)
            time_and_headlights_step = 10
            tt = sec_since_boot()

            if frame % time_and_headlights_step == 0:
                idx = (frame / time_and_headlights_step) % 4
                can_sends.append(
                    gmcan.create_adas_time_status(
                        canbus.obstacle, int((tt - self.start_time) * 60),
                        idx))
                can_sends.append(
                    gmcan.create_adas_headlights_status(canbus.obstacle))

            speed_and_accelerometer_step = 2
            if frame % speed_and_accelerometer_step == 0:
                idx = (frame / speed_and_accelerometer_step) % 4
                can_sends.append(
                    gmcan.create_adas_steering_status(canbus.obstacle, idx))
                can_sends.append(
                    gmcan.create_adas_accelerometer_speed_status(
                        canbus.obstacle, CS.v_ego, idx))

            if frame % P.ADAS_KEEPALIVE_STEP == 0:
                can_sends += gmcan.create_adas_keepalive(canbus.powertrain)

        # Show green icon when LKA torque is applied, and
        # alarming orange icon when approaching torque limit.
        # If not sent again, LKA icon disappears in about 5 seconds.
        # Conveniently, sending camera message periodically also works as a keepalive.
        lka_active = CS.lkas_status == 1
        lka_critical = lka_active and abs(actuators.steer) > 0.9
        lka_icon_status = (lka_active, lka_critical)
        if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
            or lka_icon_status != self.lka_icon_status_last:
            can_sends.append(
                gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active,
                                              lka_critical))
            self.lka_icon_status_last = lka_icon_status

        # Send chimes
        if self.chime != chime:
            duration = 0x3c

            # There is no 'repeat forever' chime command
            # TODO: Manage periodic re-issuing of chime command
            # and chime cancellation
            if chime_cnt == -1:
                chime_cnt = 10

            if chime != 0:
                can_sends.append(
                    gmcan.create_chime_command(canbus.sw_gmlan, chime,
                                               duration, chime_cnt))

            # If canceling a repeated chime, cancel command must be
            # issued for the same chime type and duration
            self.chime = chime

        sendcan.send(
            can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
Ejemplo n.º 5
0
    def update(self, c, enabled, CS, frame, actuators, hud_v_cruise,
               hud_show_lanes, hud_show_car, hud_alert):

        P = self.params

        # Send CAN commands.
        can_sends = []

        # Steering (50Hz)
        # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just received the
        # next Panda loopback confirmation in the current CS frame.
        if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last:
            self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter
        if (frame % P.STEER_STEP) == 0:
            lkas_enabled = c.active and not (
                CS.out.steerWarning
                or CS.out.steerError) and CS.out.vEgo > P.MIN_STEER_SPEED
            if lkas_enabled:
                new_steer = int(round(actuators.steer * P.STEER_MAX))
                apply_steer = apply_std_steer_torque_limits(
                    new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
                self.steer_rate_limited = new_steer != apply_steer
            else:
                apply_steer = 0

            self.apply_steer_last = apply_steer
            # GM EPS faults on any gap in received message counters. To handle transient OP/Panda safety sync issues at the
            # moment of disengaging, increment the counter based on the last message known to pass Panda safety checks.
            idx = (CS.lka_steering_cmd_counter + 1) % 4

            can_sends.append(
                gmcan.create_steering_control(self.packer_pt,
                                              CanBus.POWERTRAIN, apply_steer,
                                              idx, lkas_enabled))

        # TODO: All three conditions should not be required - really only last two?
        if CS.CP.carFingerprint not in NO_ASCM and CS.CP.openpilotLongitudinalControl and not CS.CP.pcmCruise:
            # Gas/regen and brakes - all at 25Hz
            if (frame % 4) == 0:
                if not c.active:
                    # Stock ECU sends max regen when not enabled.
                    self.apply_gas = P.MAX_ACC_REGEN
                    self.apply_brake = 0
                else:
                    self.apply_gas = int(
                        round(
                            interp(actuators.accel, P.GAS_LOOKUP_BP,
                                   P.GAS_LOOKUP_V)))
                    self.apply_brake = int(
                        round(
                            interp(actuators.accel, P.BRAKE_LOOKUP_BP,
                                   P.BRAKE_LOOKUP_V)))

                idx = (frame // 4) % 4
                # TODO: Should all instances of "enabled" be replaced with c.active?
                at_full_stop = enabled and CS.out.standstill
                near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
                can_sends.append(
                    gmcan.create_friction_brake_command(
                        self.packer_ch, CanBus.CHASSIS, self.apply_brake, idx,
                        near_stop, at_full_stop))
                can_sends.append(
                    gmcan.create_gas_regen_command(self.packer_pt,
                                                   CanBus.POWERTRAIN,
                                                   self.apply_gas, idx,
                                                   enabled, at_full_stop))

            # Send dashboard UI commands (ACC status), 25hz
            if (frame % 4) == 0:
                send_fcw = hud_alert == VisualAlert.fcw
                can_sends.append(
                    gmcan.create_acc_dashboard_command(
                        self.packer_pt, CanBus.POWERTRAIN, enabled,
                        hud_v_cruise * CV.MS_TO_KPH, hud_show_car, send_fcw))

            # Radar needs to know current speed and yaw rate (50hz),
            # and that ADAS is alive (10hz)
            time_and_headlights_step = 10
            tt = frame * DT_CTRL

            if frame % time_and_headlights_step == 0:
                idx = (frame // time_and_headlights_step) % 4
                can_sends.append(
                    gmcan.create_adas_time_status(
                        CanBus.OBSTACLE, int((tt - self.start_time) * 60),
                        idx))
                can_sends.append(
                    gmcan.create_adas_headlights_status(
                        self.packer_obj, CanBus.OBSTACLE))

            speed_and_accelerometer_step = 2
            if frame % speed_and_accelerometer_step == 0:
                idx = (frame // speed_and_accelerometer_step) % 4
                can_sends.append(
                    gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
                can_sends.append(
                    gmcan.create_adas_accelerometer_speed_status(
                        CanBus.OBSTACLE, CS.out.vEgo, idx))

            if frame % P.ADAS_KEEPALIVE_STEP == 0:
                can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)
        elif CS.CP.openpilotLongitudinalControl:
            # Gas/regen and brakes - all at 25Hz
            if (frame % 4) == 0:
                if not c.active:
                    # Stock ECU sends max regen when not enabled.
                    self.apply_gas = P.MAX_ACC_REGEN
                    self.apply_brake = 0
                else:
                    self.apply_gas = int(
                        round(
                            interp(actuators.accel, P.GAS_LOOKUP_BP,
                                   P.GAS_LOOKUP_V)))
                    self.apply_brake = int(
                        round(
                            interp(actuators.accel, P.BRAKE_LOOKUP_BP,
                                   P.BRAKE_LOOKUP_V)))

                idx = (frame // 4) % 4

                at_full_stop = enabled and CS.out.standstill
                # near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
                # VOACC based cars have brakes on PT bus - OP won't be doing VOACC for a while
                # can_sends.append(gmcan.create_friction_brake_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_brake, idx, near_stop, at_full_stop))

                if CS.CP.enableGasInterceptor:
                    # TODO: JJS Unsure if low is single pedal mode in any non-electric cars
                    singlePedalMode = CS.out.gearShifter == GearShifter.low and CS.CP.carFingerprint in EV_CAR
                    # TODO: JJS Detect saturated battery and fallback to D mode (until regen is available
                    if singlePedalMode:
                        # In L Mode, Pedal applies regen at a fixed coast-point (TODO: max regen in L mode may be different per car)
                        # This will apply to EVs in L mode.
                        # accel values below zero down to a cutoff point
                        #  that approximates the percentage of braking regen can handle should be scaled between 0 and the coast-point
                        # accell values below this point will need to be add-on future hijacked AEB
                        # TODO: Determine (or guess) at regen precentage

                        # From Felger's Bolt Bort
                        #It seems in L mode, accel / decel point is around 1/5
                        #-1-------AEB------0----regen---0.15-------accel----------+1
                        # Shrink gas request to 0.85, have it start at 0.2
                        # Shrink brake request to 0.85, first 0.15 gives regen, rest gives AEB

                        zero = 40 / 256

                        if (actuators.accel > 0.):
                            pedal_gas = clip(
                                ((1 - zero) * actuators.accel + zero), 0., 1.)
                        else:
                            pedal_gas = clip(
                                actuators.accel, 0., zero
                            )  # Make brake the same size as gas, but clip to regen
                            # aeb = actuators.brake*(1-zero)-regen # For use later, braking more than regen
                    else:
                        # In D Mode, Pedal is close to coast at 0, 100% at 1.
                        # This will apply to non-EVs and EVs in D mode.
                        # accel values below zero will need to be handled by future hijacked AEB
                        # TODO: Determine if this clipping is correct
                        pedal_gas = clip(actuators.accel, 0., 1.)

                    can_sends.append(
                        create_gas_interceptor_command(self.packer_pt,
                                                       pedal_gas, idx))
                else:
                    can_sends.append(
                        gmcan.create_gas_regen_command(self.packer_pt,
                                                       CanBus.POWERTRAIN,
                                                       self.apply_gas, idx,
                                                       enabled, at_full_stop))

        # Show green icon when LKA torque is applied, and
        # alarming orange icon when approaching torque limit.
        # If not sent again, LKA icon disappears in about 5 seconds.
        # Conveniently, sending camera message periodically also works as a keepalive.
        lka_active = CS.lkas_status == 1
        lka_critical = lka_active and abs(actuators.steer) > 0.9
        lka_icon_status = (lka_active, lka_critical)
        if frame % P.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last:
            steer_alert = hud_alert in (VisualAlert.steerRequired,
                                        VisualAlert.ldw)
            can_sends.append(
                gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active,
                                              lka_critical, steer_alert))
            self.lka_icon_status_last = lka_icon_status

        new_actuators = actuators.copy()
        new_actuators.steer = self.apply_steer_last / P.STEER_MAX
        new_actuators.gas = self.apply_gas
        new_actuators.brake = self.apply_brake

        return new_actuators, can_sends
Ejemplo n.º 6
0
    def update(self, enabled, CS, frame, actuators, hud_v_cruise,
               hud_show_lanes, hud_show_car, hud_alert):

        P = self.params

        # Send CAN commands.
        can_sends = []

        # STEER
        if (frame % P.STEER_STEP) == 0:
            lkas_enabled = (
                enabled or CS.pause_long_on_gas_press
            ) and CS.lkMode and not (
                CS.out.steerWarning or CS.out.steerError
            ) and CS.out.vEgo > P.MIN_STEER_SPEED and CS.lane_change_steer_factor > 0.
            if lkas_enabled:
                new_steer = int(
                    round(actuators.steer * P.STEER_MAX *
                          CS.lane_change_steer_factor))
                apply_steer = apply_std_steer_torque_limits(
                    new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
                self.steer_rate_limited = new_steer != apply_steer
            else:
                apply_steer = 0

            self.apply_steer_last = apply_steer
            idx = (frame // P.STEER_STEP) % 4

            can_sends.append(
                gmcan.create_steering_control(self.packer_pt,
                                              CanBus.POWERTRAIN, apply_steer,
                                              idx, lkas_enabled))

        # Gas/regen prep
        if not enabled or CS.pause_long_on_gas_press:
            # Stock ECU sends max regen when not enabled.
            apply_gas = P.MAX_ACC_REGEN
            apply_brake = 0
        else:
            apply_gas = interp(actuators.accel, P.GAS_LOOKUP_BP,
                               P.GAS_LOOKUP_V)
            apply_brake = interp(actuators.accel, P.BRAKE_LOOKUP_BP,
                                 P.BRAKE_LOOKUP_V)
            t = sec_since_boot()

            v_rel = CS.coasting_lead_v - CS.vEgo
            ttc = min(
                -CS.coasting_lead_d / v_rel if
                (CS.coasting_lead_d > 0. and v_rel < 0.) else 100., 100.)
            d_time = CS.coasting_lead_d / CS.vEgo if (CS.coasting_lead_d > 0.
                                                      and CS.vEgo > 0.
                                                      and CS.tr > 0.) else 10.

            if CS.coasting_lead_d > 0. and (ttc < CS.lead_ttc_long_gas_lockout_bp[-1] \
              or v_rel < CS.lead_v_rel_long_gas_lockout_bp[-1] \
              or CS.coasting_lead_v < CS.lead_v_long_gas_lockout_bp[-1] \
              or d_time < CS.tr * CS.lead_tr_long_gas_lockout_bp[-1]\
              or CS.coasting_lead_d < CS.lead_d_long_gas_lockout_bp[-1]):
                lead_long_gas_lockout_factor = max([
                    interp(v_rel, CS.lead_v_rel_long_gas_lockout_bp,
                           CS.lead_v_rel_long_gas_lockout_v),
                    interp(CS.coasting_lead_v, CS.lead_v_long_gas_lockout_bp,
                           CS.lead_v_long_gas_lockout_v),
                    interp(ttc, CS.lead_ttc_long_gas_lockout_bp,
                           CS.lead_ttc_long_gas_lockout_v),
                    interp(d_time / CS.tr, CS.lead_tr_long_gas_lockout_bp,
                           CS.lead_tr_long_gas_lockout_v),
                    interp(CS.coasting_lead_d, CS.lead_d_long_gas_lockout_bp,
                           CS.lead_d_long_gas_lockout_v)
                ])

                if CS.coasting_lead_d > 0. and (ttc < CS.lead_ttc_long_brake_lockout_bp[-1] \
                  or v_rel < CS.lead_v_rel_long_brake_lockout_bp[-1] \
                  or CS.coasting_lead_v < CS.lead_v_long_brake_lockout_bp[-1] \
                  or d_time < CS.tr * CS.lead_tr_long_brake_lockout_bp[-1]\
                  or CS.coasting_lead_d < CS.lead_d_long_brake_lockout_bp[-1]):
                    lead_long_brake_lockout_factor = max([
                        interp(v_rel, CS.lead_v_rel_long_brake_lockout_bp,
                               CS.lead_v_rel_long_brake_lockout_v),
                        interp(CS.coasting_lead_v,
                               CS.lead_v_long_brake_lockout_bp,
                               CS.lead_v_long_brake_lockout_v),
                        interp(ttc, CS.lead_ttc_long_brake_lockout_bp,
                               CS.lead_ttc_long_brake_lockout_v),
                        interp(d_time / CS.tr,
                               CS.lead_tr_long_brake_lockout_bp,
                               CS.lead_tr_long_brake_lockout_v),
                        interp(CS.coasting_lead_d,
                               CS.lead_d_long_brake_lockout_bp,
                               CS.lead_d_long_brake_lockout_v)
                    ])
                else:
                    lead_long_brake_lockout_factor = 0.  # 1.0 means regular braking logic is completely unaltered, 0.0 means no cruise braking
            else:
                lead_long_gas_lockout_factor = 0.  # 1.0 means regular braking logic is completely unaltered, 0.0 means no cruise braking
                lead_long_brake_lockout_factor = 0.  # 1.0 means regular braking logic is completely unaltered, 0.0 means no cruise braking

            # debug logging
            do_log = self.debug_logging and (t - self.last_debug_log_t >
                                             self.debug_log_time_step)
            if do_log:
                self.last_debug_log_t = t
                f = open("/data/openpilot/coast_debug.csv", "a")
                f.write(",".join([
                    f"{i:.1f}" if i == float else str(i) for i in [
                        t - CS.sessionInitTime, CS.coasting_long_plan,
                        CS.coasting_lead_d, CS.coasting_lead_v, CS.vEgo,
                        CS.v_cruise_kph * CV.KPH_TO_MS, CS.coasting_lead_v *
                        CV.MS_TO_MPH, CS.vEgo * CV.MS_TO_MPH, CS.v_cruise_kph *
                        CV.KPH_TO_MPH, ttc, lead_long_gas_lockout_factor,
                        lead_long_brake_lockout_factor,
                        int(apply_gas),
                        int(apply_brake),
                        (CS.one_pedal_mode_active
                         or CS.coast_one_pedal_mode_active
                         ), CS.coasting_enabled, CS.no_friction_braking
                    ]
                ]) + ",")

            if (CS.one_pedal_mode_active or CS.coast_one_pedal_mode_active):
                apply_gas = apply_gas * lead_long_gas_lockout_factor + float(
                    P.MAX_ACC_REGEN) * (1. - lead_long_gas_lockout_factor)
                time_since_brake = t - CS.one_pedal_mode_last_gas_press_t
                if CS.one_pedal_mode_active:
                    if abs(CS.angle_steers
                           ) > CS.one_pedal_angle_steers_cutoff_bp[0]:
                        one_pedal_apply_brake = interp(
                            CS.vEgo, CS.one_pedal_mode_stop_apply_brake_bp[
                                CS.one_pedal_brake_mode],
                            CS.one_pedal_mode_stop_apply_brake_v[
                                CS.one_pedal_brake_mode])
                        one_pedal_apply_brake_minus1 = interp(
                            CS.vEgo, CS.one_pedal_mode_stop_apply_brake_bp[max(
                                0, CS.one_pedal_brake_mode - 1)],
                            CS.one_pedal_mode_stop_apply_brake_v[max(
                                0, CS.one_pedal_brake_mode - 1)])
                        one_pedal_apply_brake = interp(
                            abs(CS.angle_steers),
                            CS.one_pedal_angle_steers_cutoff_bp, [
                                one_pedal_apply_brake,
                                one_pedal_apply_brake_minus1
                            ])
                    else:
                        one_pedal_apply_brake = interp(
                            CS.vEgo, CS.one_pedal_mode_stop_apply_brake_bp[
                                CS.one_pedal_brake_mode],
                            CS.one_pedal_mode_stop_apply_brake_v[
                                CS.one_pedal_brake_mode])
                    one_pedal_apply_brake *= interp(
                        CS.pitch, CS.one_pedal_pitch_brake_adjust_bp,
                        CS.one_pedal_pitch_brake_adjust_v[
                            CS.one_pedal_brake_mode])
                    one_pedal_apply_brake = min(one_pedal_apply_brake,
                                                float(P.BRAKE_LOOKUP_V[0]))
                    one_pedal_apply_brake *= interp(
                        time_since_brake, CS.one_pedal_mode_ramp_time_bp,
                        CS.one_pedal_mode_ramp_time_v
                    ) if CS.one_pedal_brake_mode < 2 else 1.
                else:
                    one_pedal_apply_brake = 0.

                # ramp braking
                if CS.one_pedal_mode_active_last and time_since_brake > CS.one_pedal_mode_ramp_time_bp[
                        -1]:
                    if CS.one_pedal_mode_apply_brake != one_pedal_apply_brake:
                        if CS.one_pedal_mode_ramp_mode_last != CS.one_pedal_brake_mode:
                            # brake mode changed, so need to calculate new step based on the old and new modes
                            old_apply_brake = interp(
                                CS.vEgo, CS.one_pedal_mode_stop_apply_brake_bp[
                                    CS.one_pedal_mode_ramp_mode_last],
                                CS.one_pedal_mode_stop_apply_brake_v[
                                    CS.one_pedal_mode_ramp_mode_last])
                            CS.one_pedal_mode_ramp_time_step = (
                                one_pedal_apply_brake - old_apply_brake
                            ) / CS.one_pedal_mode_ramp_duration
                        if CS.one_pedal_mode_apply_brake < one_pedal_apply_brake:
                            if CS.one_pedal_mode_ramp_time_step < 0.:
                                CS.one_pedal_mode_ramp_time_step *= -1.
                            CS.one_pedal_mode_apply_brake = max(
                                one_pedal_apply_brake,
                                CS.one_pedal_mode_apply_brake +
                                CS.one_pedal_mode_ramp_time_step *
                                (t - CS.one_pedal_mode_ramp_t_last))
                        else:
                            if CS.one_pedal_mode_ramp_time_step > 0.:
                                CS.one_pedal_mode_ramp_time_step *= -1.
                            CS.one_pedal_mode_apply_brake = min(
                                one_pedal_apply_brake,
                                CS.one_pedal_mode_apply_brake +
                                CS.one_pedal_mode_ramp_time_step *
                                (t - CS.one_pedal_mode_ramp_t_last))
                        one_pedal_apply_brake = CS.one_pedal_mode_apply_brake
                else:
                    CS.one_pedal_mode_apply_brake = one_pedal_apply_brake
                    CS.one_pedal_mode_active_last = True
                CS.one_pedal_mode_ramp_t_last = t
                CS.one_pedal_mode_ramp_mode_last = CS.one_pedal_brake_mode

                if CS.one_pedal_mode_op_braking_allowed and CS.coasting_long_plan not in [
                        'cruise', 'limit'
                ]:
                    apply_brake = max(
                        one_pedal_apply_brake,
                        apply_brake * lead_long_brake_lockout_factor)
                else:
                    apply_brake = one_pedal_apply_brake

            elif CS.coasting_enabled and lead_long_brake_lockout_factor < 1.:
                if CS.coasting_long_plan in [
                        'cruise', 'limit'
                ] and apply_gas < P.ZERO_GAS or apply_brake > 0.:
                    check_speed_ms = (CS.speed_limit if CS.speed_limit_active
                                      and CS.speed_limit < CS.v_cruise_kph else
                                      CS.v_cruise_kph) * CV.KPH_TO_MS
                    if apply_brake > 0.:
                        coasting_over_speed_vEgo_BP = [
                            interp(CS.vEgo, CS.coasting_over_speed_vEgo_BP_BP,
                                   CS.coasting_over_speed_vEgo_BP[0]),
                            interp(CS.vEgo, CS.coasting_over_speed_vEgo_BP_BP,
                                   CS.coasting_over_speed_vEgo_BP[1])
                        ]
                        over_speed_factor = interp(
                            CS.vEgo / check_speed_ms,
                            coasting_over_speed_vEgo_BP, [0., 1.]) if (
                                check_speed_ms > 0. and
                                CS.coasting_brake_over_speed_enabled) else 0.
                        over_speed_brake = apply_brake * over_speed_factor
                        apply_brake = max([
                            apply_brake * lead_long_brake_lockout_factor,
                            over_speed_brake
                        ])
                    if apply_gas < P.ZERO_GAS and lead_long_gas_lockout_factor < 1.:
                        coasting_over_speed_vEgo_BP = [
                            interp(CS.vEgo, CS.coasting_over_speed_vEgo_BP_BP,
                                   CS.coasting_over_speed_regen_vEgo_BP[0]),
                            interp(CS.vEgo, CS.coasting_over_speed_vEgo_BP_BP,
                                   CS.coasting_over_speed_regen_vEgo_BP[1])
                        ]
                        over_speed_factor = interp(
                            CS.vEgo / check_speed_ms,
                            coasting_over_speed_vEgo_BP, [0., 1.]) if (
                                check_speed_ms > 0 and
                                CS.coasting_brake_over_speed_enabled) else 0.
                        coast_apply_gas = int(
                            round(
                                float(P.ZERO_GAS) - over_speed_factor *
                                (P.ZERO_GAS - apply_gas)))
                        apply_gas = apply_gas * lead_long_gas_lockout_factor + coast_apply_gas * (
                            1. - lead_long_gas_lockout_factor)
            elif CS.no_friction_braking and lead_long_brake_lockout_factor < 1.:
                if CS.coasting_long_plan in ['cruise', 'limit'
                                             ] and apply_brake > 0.:
                    apply_brake *= lead_long_brake_lockout_factor
            apply_gas = int(round(apply_gas))
            apply_brake = int(round(apply_brake))

            CS.one_pedal_mode_active_last = CS.one_pedal_mode_active

            if do_log:
                f.write(",".join([str(i)
                                  for i in [apply_gas, apply_brake]]) + "\n")
                f.close()

        if CS.showBrakeIndicator:
            CS.apply_brake_percent = int(
                interp(apply_brake, [
                    float(P.BRAKE_LOOKUP_V[-1]),
                    float(P.BRAKE_LOOKUP_V[0])
                ], [0., 100.])) if CS.vEgo > 0.1 else 0

        # Gas/regen and brakes - all at 25Hz
        if (frame % 4) == 0:
            idx = (frame // 4) % 4

            if CS.cruiseMain and not enabled and CS.autoHold and CS.autoHoldActive and not CS.out.gasPressed and CS.out.gearShifter == 'drive' and CS.out.vEgo < 0.01 and not CS.regenPaddlePressed:
                # Auto Hold State
                car_stopping = apply_gas < P.ZERO_GAS
                standstill = CS.pcm_acc_status == AccState.STANDSTILL

                at_full_stop = standstill and car_stopping
                near_stop = (CS.out.vEgo <
                             P.NEAR_STOP_BRAKE_PHASE) and car_stopping
                can_sends.append(
                    gmcan.create_friction_brake_command(
                        self.packer_ch, CanBus.CHASSIS, apply_brake, idx,
                        near_stop, at_full_stop))
                CS.autoHoldActivated = True

            else:
                if CS.pause_long_on_gas_press:
                    at_full_stop = False
                    near_stop = False
                    car_stopping = False
                    standstill = False
                else:
                    car_stopping = apply_gas < P.ZERO_GAS
                    standstill = CS.pcm_acc_status == AccState.STANDSTILL
                    at_full_stop = enabled and standstill and car_stopping
                    near_stop = enabled and (
                        CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) and car_stopping

                can_sends.append(
                    gmcan.create_friction_brake_command(
                        self.packer_ch, CanBus.CHASSIS, apply_brake, idx,
                        near_stop, at_full_stop))
                CS.autoHoldActivated = False

                # Auto-resume from full stop by resetting ACC control
                acc_enabled = enabled

                if standstill and not car_stopping:
                    acc_enabled = False

                can_sends.append(
                    gmcan.create_gas_regen_command(self.packer_pt,
                                                   CanBus.POWERTRAIN,
                                                   apply_gas, idx, acc_enabled,
                                                   at_full_stop))

        # Send dashboard UI commands (ACC status), 25hz
        if (frame % 4) == 0:
            send_fcw = hud_alert == VisualAlert.fcw
            follow_level = CS.get_follow_level()
            can_sends.append(
                gmcan.create_acc_dashboard_command(self.packer_pt,
                                                   CanBus.POWERTRAIN, enabled,
                                                   hud_v_cruise * CV.MS_TO_KPH,
                                                   hud_show_car, follow_level,
                                                   send_fcw))

        # Radar needs to know current speed and yaw rate (50hz),
        # and that ADAS is alive (10hz)
        time_and_headlights_step = 10
        tt = frame * DT_CTRL

        if frame % time_and_headlights_step == 0:
            idx = (frame // time_and_headlights_step) % 4
            can_sends.append(
                gmcan.create_adas_time_status(CanBus.OBSTACLE,
                                              int((tt - self.start_time) * 60),
                                              idx))
            can_sends.append(
                gmcan.create_adas_headlights_status(self.packer_obj,
                                                    CanBus.OBSTACLE))

        speed_and_accelerometer_step = 2
        if frame % speed_and_accelerometer_step == 0:
            idx = (frame // speed_and_accelerometer_step) % 4
            can_sends.append(
                gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
            can_sends.append(
                gmcan.create_adas_accelerometer_speed_status(
                    CanBus.OBSTACLE, CS.out.vEgo, idx))

        if frame % P.ADAS_KEEPALIVE_STEP == 0:
            can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)

        # Show green icon when LKA torque is applied, and
        # alarming orange icon when approaching torque limit.
        # If not sent again, LKA icon disappears in about 5 seconds.
        # Conveniently, sending camera message periodically also works as a keepalive.
        lka_active = CS.lkas_status == 1
        lka_critical = lka_active and abs(actuators.steer) > 0.9
        lka_icon_status = (lka_active, lka_critical)
        if frame % P.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last:
            steer_alert = hud_alert in [
                VisualAlert.steerRequired, VisualAlert.ldw
            ]
            can_sends.append(
                gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active,
                                              lka_critical, steer_alert))
            self.lka_icon_status_last = lka_icon_status

        return can_sends
Ejemplo n.º 7
0
    def update(self, enabled, CS, frame, actuators, \
               hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt, hud_alert):

        P = self.params

        # Send CAN commands.
        can_sends = []
        canbus = self.canbus

        alert_out = process_hud_alert(hud_alert)
        steer = alert_out

        ### STEER ###

        if (frame % P.STEER_STEP) == 0:
            lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > P.MIN_STEER_SPEED
            if lkas_enabled:
                apply_steer = actuators.steer * P.STEER_MAX
                apply_steer = apply_std_steer_torque_limits(
                    apply_steer, self.apply_steer_last, CS.steer_torque_driver,
                    P)
            else:
                apply_steer = 0

            self.apply_steer_last = apply_steer
            idx = (frame // P.STEER_STEP) % 4

            if self.car_fingerprint in SUPERCRUISE_CARS:
                can_sends += gmcan.create_steering_control_ct6(
                    self.packer_pt, canbus, apply_steer, CS.v_ego, idx,
                    lkas_enabled)
            else:
                can_sends.append(
                    gmcan.create_steering_control(self.packer_pt,
                                                  canbus.powertrain,
                                                  apply_steer, idx,
                                                  lkas_enabled))

        ### GAS/BRAKE ###

        if self.car_fingerprint not in SUPERCRUISE_CARS:
            # no output if not enabled, but keep sending keepalive messages
            # treat pedals as one
            final_pedal = actuators.gas - actuators.brake

            # *** apply pedal hysteresis ***
            final_brake, self.brake_steady = actuator_hystereses(
                final_pedal, self.pedal_steady)

            if not enabled:
                # Stock ECU sends max regen when not enabled.
                apply_gas = P.MAX_ACC_REGEN
                apply_brake = 0
            else:
                apply_gas = int(
                    round(interp(final_pedal, P.GAS_LOOKUP_BP,
                                 P.GAS_LOOKUP_V)))
                apply_brake = int(
                    round(
                        interp(final_pedal, P.BRAKE_LOOKUP_BP,
                               P.BRAKE_LOOKUP_V)))

            # Gas/regen and brakes - all at 25Hz
            if (frame % 4) == 0:
                idx = (frame // 4) % 4

                at_full_stop = enabled and CS.standstill
                near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE)
                can_sends.append(
                    gmcan.create_friction_brake_command(
                        self.packer_ch, canbus.chassis, apply_brake, idx,
                        near_stop, at_full_stop))

                at_full_stop = enabled and CS.standstill
                can_sends.append(
                    gmcan.create_gas_regen_command(self.packer_pt,
                                                   canbus.powertrain,
                                                   apply_gas, idx, enabled,
                                                   at_full_stop))

            # Send dashboard UI commands (ACC status), 25hz
            if (frame % 4) == 0:
                can_sends.append(
                    gmcan.create_acc_dashboard_command(
                        self.packer_pt, canbus.powertrain, enabled,
                        hud_v_cruise * CV.MS_TO_KPH, hud_show_car))

            # Radar needs to know current speed and yaw rate (50hz),
            # and that ADAS is alive (10hz)
            time_and_headlights_step = 10
            tt = sec_since_boot()

            if frame % time_and_headlights_step == 0:
                idx = (frame // time_and_headlights_step) % 4
                can_sends.append(
                    gmcan.create_adas_time_status(
                        canbus.obstacle, int((tt - self.start_time) * 60),
                        idx))
                can_sends.append(
                    gmcan.create_adas_headlights_status(canbus.obstacle))

            speed_and_accelerometer_step = 2
            if frame % speed_and_accelerometer_step == 0:
                idx = (frame // speed_and_accelerometer_step) % 4
                can_sends.append(
                    gmcan.create_adas_steering_status(canbus.obstacle, idx))
                can_sends.append(
                    gmcan.create_adas_accelerometer_speed_status(
                        canbus.obstacle, CS.v_ego, idx))

            if frame % P.ADAS_KEEPALIVE_STEP == 0:
                can_sends += gmcan.create_adas_keepalive(canbus.powertrain)

            # Show green icon when LKA torque is applied, and
            # alarming orange icon when approaching torque limit.
            # If not sent again, LKA icon disappears in about 5 seconds.
            # Conveniently, sending camera message periodically also works as a keepalive.
            lka_active = CS.lkas_status == 1
            lka_critical = lka_active and abs(actuators.steer) > 0.9
            lka_icon_status = (lka_active, lka_critical)
            if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
                or lka_icon_status != self.lka_icon_status_last:
                can_sends.append(
                    gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active,
                                                  lka_critical, steer))
                self.lka_icon_status_last = lka_icon_status

        # Send chimes
        if self.chime != chime:
            duration = 0x3c

            # There is no 'repeat forever' chime command
            # TODO: Manage periodic re-issuing of chime command
            # and chime cancellation
            if chime_cnt == -1:
                chime_cnt = 10

            if chime != 0:
                can_sends.append(
                    gmcan.create_chime_command(canbus.sw_gmlan, chime,
                                               duration, chime_cnt))

            # If canceling a repeated chime, cancel command must be
            # issued for the same chime type and duration
            self.chime = chime

        return can_sends
Ejemplo n.º 8
0
    def update(self, enabled, CS, frame, actuators, \
               hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):

        P = self.params
        # Send CAN commands.
        can_sends = []

        alert_out = process_hud_alert(hud_alert)
        steer = alert_out

        ### STEER ###

        if (frame % P.STEER_STEP) == 0:
            lkas_enabled = enabled and not CS.steer_warning and CS.lkMode and CS.out.vEgo > P.MIN_STEER_SPEED
            if lkas_enabled:
                new_steer = actuators.steer * P.STEER_MAX
                apply_steer = apply_std_steer_torque_limits(
                    new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
                self.steer_rate_limited = new_steer != apply_steer
            else:
                apply_steer = 0

            self.apply_steer_last = apply_steer
            idx = (frame // P.STEER_STEP) % 4

            if self.car_fingerprint in SUPERCRUISE_CARS:
                can_sends += gmcan.create_steering_control_ct6(
                    self.packer_pt, CanBus, apply_steer, CS.out.vEgo, idx,
                    lkas_enabled)
            else:
                can_sends.append(
                    gmcan.create_steering_control(self.packer_pt,
                                                  CanBus.POWERTRAIN,
                                                  apply_steer, idx,
                                                  lkas_enabled))

        ### GAS/BRAKE ###

        if self.car_fingerprint not in SUPERCRUISE_CARS:
            # no output if not enabled, but keep sending keepalive messages
            # treat pedals as one
            final_pedal = actuators.gas - actuators.brake

            # *** apply pedal hysteresis ***
            final_brake, self.brake_steady = actuator_hystereses(
                final_pedal, self.pedal_steady)

            if not enabled:
                # Stock ECU sends max regen when not enabled.
                apply_gas = P.MAX_ACC_REGEN
                apply_brake = 0
            else:
                apply_gas = int(
                    round(interp(final_pedal, P.GAS_LOOKUP_BP,
                                 P.GAS_LOOKUP_V)))
                apply_brake = int(
                    round(
                        interp(final_pedal, P.BRAKE_LOOKUP_BP,
                               P.BRAKE_LOOKUP_V)))

            # Gas/regen and brakes - all at 25Hz
            if (frame % 4) == 0:
                idx = (frame // 4) % 4

                car_stopping = apply_gas < P.ZERO_GAS
                standstill = CS.pcm_acc_status == AccState.STANDSTILL
                at_full_stop = enabled and standstill and car_stopping
                near_stop = enabled and (
                    CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) and car_stopping
                can_sends.append(
                    gmcan.create_friction_brake_command(
                        self.packer_ch, CanBus.CHASSIS, apply_brake, idx,
                        near_stop, at_full_stop))

                # Auto-resume from full stop by resetting ACC control
                acc_enabled = enabled
                if standstill and not car_stopping:
                    acc_enabled = False

                can_sends.append(
                    gmcan.create_gas_regen_command(self.packer_pt,
                                                   CanBus.POWERTRAIN,
                                                   apply_gas, idx, acc_enabled,
                                                   at_full_stop))

            # Send dashboard UI commands (ACC status), 25hz
            follow_level = CS.get_follow_level()
            if (frame % 4) == 0:

                send_fcw = 0
                if self.fcw_count > 0:
                    self.fcw_count -= 1
                    send_fcw = 0x3
                can_sends.append(
                    gmcan.create_acc_dashboard_command(
                        self.packer_pt, CanBus.POWERTRAIN, enabled,
                        hud_v_cruise * CV.MS_TO_KPH, hud_show_car,
                        follow_level, send_fcw))

            # Radar needs to know current speed and yaw rate (50hz),
            # and that ADAS is alive (10hz)
            time_and_headlights_step = 10
            tt = frame * DT_CTRL

            if frame % time_and_headlights_step == 0:
                idx = (frame // time_and_headlights_step) % 4
                can_sends.append(
                    gmcan.create_adas_time_status(
                        CanBus.OBSTACLE, int((tt - self.start_time) * 60),
                        idx))
                can_sends.append(
                    gmcan.create_adas_headlights_status(CanBus.OBSTACLE))

            speed_and_accelerometer_step = 2
            if frame % speed_and_accelerometer_step == 0:
                idx = (frame // speed_and_accelerometer_step) % 4
                can_sends.append(
                    gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
                can_sends.append(
                    gmcan.create_adas_accelerometer_speed_status(
                        CanBus.OBSTACLE, CS.out.vEgo, idx))

            if frame % P.ADAS_KEEPALIVE_STEP == 0:
                can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)

            # Show green icon when LKA torque is applied, and
            # alarming orange icon when approaching torque limit.
            # If not sent again, LKA icon disappears in about 5 seconds.
            # Conveniently, sending camera message periodically also works as a keepalive.
            lka_active = CS.lkas_status == 1
            lka_critical = lka_active and abs(actuators.steer) > 0.9
            lka_icon_status = (lka_active, lka_critical)
            if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
                or lka_icon_status != self.lka_icon_status_last:
                can_sends.append(
                    gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active,
                                                  lka_critical, steer))
                self.lka_icon_status_last = lka_icon_status

        return can_sends
Ejemplo n.º 9
0
    def update(self, sendcan, enabled, CS, frame, actuators, \
               hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt):
        """ Controls thread """

        P = self.params

        # Send CAN commands.
        can_sends = []
        canbus = self.canbus

        ### STEER ###

        if (frame % P.STEER_STEP) == 0:
            final_steer = actuators.steer if enabled else 0.
            apply_steer = final_steer * P.STEER_MAX

            apply_steer = apply_std_steer_torque_limits(
                apply_steer, self.apply_steer_last, CS.steer_torque_driver, P)

            lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > 3.

            if not lkas_enabled:
                apply_steer = 0

            self.apply_steer_last = apply_steer
            idx = (frame / P.STEER_STEP) % 4

            if self.car_fingerprint == CAR.VOLT:
                can_sends.append(
                    gmcan.create_steering_control(self.packer_pt,
                                                  canbus.powertrain,
                                                  apply_steer, idx,
                                                  lkas_enabled))
            if self.car_fingerprint == CAR.CADILLAC_CT6:
                can_sends += gmcan.create_steering_control_ct6(
                    self.packer_pt, canbus, apply_steer, CS.v_ego, idx,
                    lkas_enabled)

        ### GAS/BRAKE ###

        if self.car_fingerprint == CAR.VOLT:
            # no output if not enabled, but keep sending keepalive messages
            # threat pedals as one
            final_pedal = actuators.gas - actuators.brake

            # *** apply pedal hysteresis ***
            final_brake, self.brake_steady = actuator_hystereses(
                final_pedal, self.pedal_steady)

            if not enabled:
                apply_gas = P.MAX_ACC_REGEN  # TODO: do we really need to send max regen when not enabled?
                apply_brake = 0
            else:
                apply_gas = int(
                    round(interp(final_pedal, P.GAS_LOOKUP_BP,
                                 P.GAS_LOOKUP_V)))
                apply_brake = int(
                    round(
                        interp(final_pedal, P.BRAKE_LOOKUP_BP,
                               P.BRAKE_LOOKUP_V)))

            # Gas/regen and brakes - all at 25Hz
            if (frame % 4) == 0:
                idx = (frame / 4) % 4

                at_full_stop = enabled and CS.standstill
                near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE)
                can_sends.append(
                    gmcan.create_friction_brake_command(
                        self.packer_ch, canbus.chassis, apply_brake, idx,
                        near_stop, at_full_stop))

                at_full_stop = enabled and CS.standstill
                can_sends.append(
                    gmcan.create_gas_regen_command(self.packer_pt,
                                                   canbus.powertrain,
                                                   apply_gas, idx, enabled,
                                                   at_full_stop))

            # Send dashboard UI commands (ACC status), 25hz
            if (frame % 4) == 0:
                can_sends.append(
                    gmcan.create_acc_dashboard_command(
                        canbus.powertrain, enabled,
                        hud_v_cruise / CV.MS_TO_KPH, hud_show_car))

            # Radar needs to know current speed and yaw rate (50hz),
            # and that ADAS is alive (10hz)
            time_and_headlights_step = 10
            tt = sec_since_boot()

            if frame % time_and_headlights_step == 0:
                idx = (frame / time_and_headlights_step) % 4
                can_sends.append(
                    gmcan.create_adas_time_status(
                        canbus.obstacle, int((tt - self.start_time) * 60),
                        idx))
                can_sends.append(
                    gmcan.create_adas_headlights_status(canbus.obstacle))

            speed_and_accelerometer_step = 2
            if frame % speed_and_accelerometer_step == 0:
                idx = (frame / speed_and_accelerometer_step) % 4
                can_sends.append(
                    gmcan.create_adas_steering_status(canbus.obstacle, idx))
                can_sends.append(
                    gmcan.create_adas_accelerometer_speed_status(
                        canbus.obstacle, CS.v_ego, idx))

            # Send ADAS keepalive, 10hz
            if frame % P.ADAS_KEEPALIVE_STEP == 0:
                can_sends += gmcan.create_adas_keepalive(canbus.powertrain)

        # Send chimes
        if self.chime != chime:
            duration = 0x3c

            # There is no 'repeat forever' chime command
            # TODO: Manage periodic re-issuing of chime command
            # and chime cancellation
            if chime_cnt == -1:
                chime_cnt = 10

            if chime != 0:
                can_sends.append(
                    gmcan.create_chime_command(canbus.sw_gmlan, chime,
                                               duration, chime_cnt))

            # If canceling a repeated chime, cancel command must be
            # issued for the same chime type and duration
            self.chime = chime

        sendcan.send(
            can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())