diff --git a/panda/board/get_sdk.sh b/panda/board/get_sdk.sh old mode 100755 new mode 100644 diff --git a/panda/board/get_sdk_mac.sh b/panda/board/get_sdk_mac.sh old mode 100755 new mode 100644 diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h index 75c3e2439d21a1..f7009410732c77 100644 --- a/panda/board/safety/safety_tesla.h +++ b/panda/board/safety/safety_tesla.h @@ -8,31 +8,30 @@ // brake rising edge // brake > 0mph - // 2m/s are added to be less restrictive const struct lookup_t TESLA_LOOKUP_ANGLE_RATE_UP = { - {2., 7., 17.}, - {5., .8, .15}}; + {2., 7., 17.}, + {5., .8, .25}}; const struct lookup_t TESLA_LOOKUP_ANGLE_RATE_DOWN = { - {2., 7., 17.}, - {5., 3.5, .4}}; + {2., 7., 17.}, + {5., 3.5, .8}}; const struct lookup_t TESLA_LOOKUP_MAX_ANGLE = { - {2., 29., 38.}, - {410.,92.,36.}}; + {2., 29., 38.}, + {410., 92., 36.}}; +const int TESLA_RT_INTERVAL = 250000; // 250ms between real time checks -struct sample_t tesla_angle_meas; // last 3 steer angles +struct sample_t tesla_angle_meas; // last 3 steer angles // state of angle limits -int tesla_desired_angle_last = 0; // last desired steer angle -int16_t tesla_rt_angle_last = 0.; // last real time angle +int tesla_desired_angle_last = 0; // last desired steer angle +int16_t tesla_rt_angle_last = 0.; // last real time angle uint32_t tesla_ts_angle_last = 0; - int tesla_controls_allowed_last = 0; - +int steer_allowed = 1; int tesla_brake_prev = 0; int tesla_gas_prev = 0; @@ -44,22 +43,28 @@ int eac_status = 0; int tesla_ignition_started = 0; // interp function that holds extreme values -float tesla_interpolate(struct lookup_t xy, float x) { +float tesla_interpolate(struct lookup_t xy, float x) +{ int size = sizeof(xy.x) / sizeof(xy.x[0]); // x is lower than the first point in the x array. Return the first point - if (x <= xy.x[0]) { + if (x <= xy.x[0]) + { return xy.y[0]; - - } else { + } + else + { // find the index such that (xy.x[i] <= x < xy.x[i+1]) and linearly interp - for (int i=0; i < size-1; i++) { - if (x < xy.x[i+1]) { + for (int i = 0; i < size - 1; i++) + { + if (x < xy.x[i + 1]) + { float x0 = xy.x[i]; float y0 = xy.y[i]; - float dx = xy.x[i+1] - x0; - float dy = xy.y[i+1] - y0; + float dx = xy.x[i + 1] - x0; + float dy = xy.y[i + 1] - y0; // dx should not be zero as xy.x is supposed ot be monotonic - if (dx <= 0.) dx = 0.0001; + if (dx <= 0.) + dx = 0.0001; return dy * (x - x0) / dx + y0; } } @@ -68,49 +73,58 @@ float tesla_interpolate(struct lookup_t xy, float x) { } } - -static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { +static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) +{ set_gmlan_digital_output(GMLAN_HIGH); reset_gmlan_switch_timeout(); //we're still in tesla safety mode, reset the timeout counter and make sure our output is enabled - + //int bus_number = (to_push->RDTR >> 4) & 0xFF; uint32_t addr; - if (to_push->RIR & 4) { + if (to_push->RIR & 4) + { // Extended // Not looked at, but have to be separated // to avoid address collision addr = to_push->RIR >> 3; - } else { + } + else + { // Normal addr = to_push->RIR >> 21; } - // Record the current car time in current_car_time (for use with double-pulling cruise stalk) - if (addr == 0x318) { + // Record the current car time in current_car_time (for use with double-pulling cruise stalk) + if (addr == 0x318) + { int hour = (to_push->RDLR & 0x1F000000) >> 24; int minute = (to_push->RDHR & 0x3F00) >> 8; int second = (to_push->RDLR & 0x3F0000) >> 16; current_car_time = (hour * 3600) + (minute * 60) + second; } - - if (addr == 0x45) { + + if (addr == 0x45) + { // 6 bits starting at position 0 int lever_position = (to_push->RDLR & 0x3F); - if (lever_position == 2) { // pull forward + if (lever_position == 2) + { // pull forward // activate openpilot // TODO: uncomment the if to use double pull to activate //if (current_car_time <= time_at_last_stalk_pull + 1 && current_car_time != -1 && time_at_last_stalk_pull != -1) { - controls_allowed = 1; + controls_allowed = 1; //} time_at_last_stalk_pull = current_car_time; - } else if (lever_position == 1) { // push towards the back + } + else if (lever_position == 1) + { // push towards the back // deactivate openpilot controls_allowed = 0; } - } - + } + // Detect drive rail on (ignition) (start recording) - if (addr == 0x348) { + if (addr == 0x348) + { // GTW_status int drive_rail_on = (to_push->RDLR & 0x0001); tesla_ignition_started = drive_rail_on == 1; @@ -118,59 +132,73 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on brake press // DI_torque2::DI_brakePedal 0x118 - if (addr == 0x118) { + if (addr == 0x118) + { // 1 bit at position 16 - if (((to_push->RDLR & 0x8000)) >> 15 == 1) { + if (((to_push->RDLR & 0x8000)) >> 15 == 1) + { //disable break cancel by commenting line below //controls_allowed = 0; } //get vehicle speed in m/2. Tesla gives MPH - tesla_speed = ((((((to_push->RDLR >> 24) & 0x0F) << 8) + (( to_push->RDLR >> 16) & 0xFF)) * 0.05 -25)*1.609/3.6); - if (tesla_speed < 0) { + tesla_speed = ((((((to_push->RDLR >> 24) & 0x0F) << 8) + ((to_push->RDLR >> 16) & 0xFF)) * 0.05 - 25) * 1.609 / 3.6); + if (tesla_speed < 0) + { tesla_speed = 0; } - } - + } + // exit controls on EPAS error // EPAS_sysStatus::EPAS_eacStatus 0x370 - if (addr == 0x370) { + if (addr == 0x370) + { // if EPAS_eacStatus is not 1 or 2, disable control eac_status = ((to_push->RDHR >> 21)) & 0x7; - if ((controls_allowed == 1) && (eac_status != 1) && (eac_status != 2)) { + // For human steering override we must not disable controls when eac_status == 0 + // Additional safety: we could only allow eac_status == 0 when we have human steerign allowed + if ((controls_allowed == 1) && (eac_status != 0) && (eac_status != 1) && (eac_status != 2)) + { controls_allowed = 0; puts("EPAS error! \n"); } - } + } //get latest steering wheel angle - if (addr == 0x00E) { - int angle_meas_now = (int) ((((to_push->RDLR & 0x3F) <<8) + ((to_push->RDLR >> 8) & 0xFF)) * 0.1 - 819.2); + if (addr == 0x00E) + { + int angle_meas_now = (int)((((to_push->RDLR & 0x3F) << 8) + ((to_push->RDLR >> 8) & 0xFF)) * 0.1 - 819.2); uint32_t ts = TIM2->CNT; - uint32_t ts_elapsed = get_ts_elapsed(ts, ts_angle_last); + uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_ts_angle_last); // *** angle real time check // add 1 to not false trigger the violation and multiply by 25 since the check is done every 250 ms and steer angle is updated at 100Hz int rt_delta_angle_up = ((int)((tesla_interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, tesla_speed) * 25. + 1.))); - int rt_delta_angle_down = ((int)( (tesla_interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, tesla_speed) * 25. + 1.))); - int highest_rt_angle = tesla_rt_angle_last + (tesla_rt_angle_last > 0 ? rt_delta_angle_up:rt_delta_angle_down); - int lowest_rt_angle = tesla_rt_angle_last - (tesla_rt_angle_last > 0 ? rt_delta_angle_down:rt_delta_angle_up); + int rt_delta_angle_down = ((int)((tesla_interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, tesla_speed) * 25. + 1.))); + int highest_rt_angle = tesla_rt_angle_last + (tesla_rt_angle_last > 0 ? rt_delta_angle_up : rt_delta_angle_down); + int lowest_rt_angle = tesla_rt_angle_last - (tesla_rt_angle_last > 0 ? rt_delta_angle_down : rt_delta_angle_up); - if ((ts_elapsed > RT_INTERVAL) || (controls_allowed && !tesla_controls_allowed_last)) { - tesla_rt_angle_last = angle_meas_now; + if ((ts_elapsed > TESLA_RT_INTERVAL) || (controls_allowed && !tesla_controls_allowed_last)) + { + tesla_rt_angle_last = angle_meas_now; tesla_ts_angle_last = ts; } // update array of samples update_sample(&tesla_angle_meas, angle_meas_now); - // check for violation; - if (max_limit_check(angle_meas_now, highest_rt_angle, lowest_rt_angle)) { - controls_allowed = 0; - puts("RT Angle Error! \n"); + if (max_limit_check(angle_meas_now, highest_rt_angle, lowest_rt_angle)) + { + // We should not be able to STEER under these conditions + // Other sending is fine (to allow human override) + steer_allowed = 0; + puts("WARN: RT Angle - No steer allowed! \n"); + } + else + { + steer_allowed = 1; } - tesla_controls_allowed_last = controls_allowed; - + tesla_controls_allowed_last = controls_allowed; } } @@ -180,81 +208,109 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // else // block all commands that produce actuation -static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { +static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) +{ uint32_t addr; int angle_raw; int desired_angle; addr = to_send->RIR >> 21; - + // do not transmit CAN message if steering angle too high // DAS_steeringControl::DAS_steeringAngleRequest - if (addr == 0x488) { + if (addr == 0x488) + { angle_raw = ((to_send->RDLR & 0x7F) << 8) + ((to_send->RDLR & 0xFF00) >> 8); desired_angle = angle_raw * 0.1 - 1638.35; int16_t violation = 0; + int st_enabled = (to_send->RDLR & 0x400000) >> 22; - if (controls_allowed) { - - // add 1 to not false trigger the violation - int delta_angle_up = (int) (tesla_interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, tesla_speed)* 25. + 1.); - int delta_angle_down = (int) (tesla_interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, tesla_speed)* 25. + 1.); - int highest_desired_angle = tesla_desired_angle_last + (tesla_desired_angle_last > 0 ? delta_angle_up:delta_angle_down); - int lowest_desired_angle = tesla_desired_angle_last - (tesla_desired_angle_last > 0 ? delta_angle_down:delta_angle_up); - int TESLA_MAX_ANGLE = (int) (tesla_interpolate(TESLA_LOOKUP_MAX_ANGLE, tesla_speed) +1.); - - if (max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle)){ - violation = 1; - controls_allowed = 0; - puts("Angle limit - delta! \n"); - } - if (max_limit_check(desired_angle, TESLA_MAX_ANGLE, -TESLA_MAX_ANGLE)) { - violation =1; - controls_allowed = 0; - puts("Angle limit - max! \n"); - } - } - -// makes no sense to have angle limits when not engaged -// if ((!controls_allowed) && max_limit_check(desired_angle, tesla_angle_meas.max + 1, tesla_angle_meas.min -1)) { -// violation = 1; -// puts("Angle limit when not engaged! \n"); -// } + if (st_enabled == 0) { + //steering is not enabled, do not check angles and do send + tesla_desired_angle_last = desired_angle; + return true; + } + + if (controls_allowed) + { + if (steer_allowed) + { + + // add 1 to not false trigger the violation + int delta_angle_up = (int)(tesla_interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, tesla_speed) * 25. + 1.); + int delta_angle_down = (int)(tesla_interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, tesla_speed) * 25. + 1.); + int highest_desired_angle = tesla_desired_angle_last + (tesla_desired_angle_last > 0 ? delta_angle_up : delta_angle_down); + int lowest_desired_angle = tesla_desired_angle_last - (tesla_desired_angle_last > 0 ? delta_angle_down : delta_angle_up); + int TESLA_MAX_ANGLE = (int)(tesla_interpolate(TESLA_LOOKUP_MAX_ANGLE, tesla_speed) + 1.); + + if (max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle)) + { + violation = 1; + controls_allowed = 0; + puts("Angle limit - delta! \n"); + } + if (max_limit_check(desired_angle, TESLA_MAX_ANGLE, -TESLA_MAX_ANGLE)) + { + violation = 1; + controls_allowed = 0; + puts("Angle limit - max! \n"); + } + } + else + { + violation = 1; + controls_allowed = 0; + puts("Steering commads disallowed"); + } + } + + // makes no sense to have angle limits when not engaged + // if ((!controls_allowed) && max_limit_check(desired_angle, tesla_angle_meas.max + 1, tesla_angle_meas.min -1)) { + // violation = 1; + // puts("Angle limit when not engaged! \n"); + // } tesla_desired_angle_last = desired_angle; - if (violation) { + if (violation) + { return false; } - return true; - } + return true; + } return true; } -static int tesla_tx_lin_hook(int lin_num, uint8_t *data, int len) { +static int tesla_tx_lin_hook(int lin_num, uint8_t *data, int len) +{ // LIN is not used on the Tesla return false; } -static void tesla_init(int16_t param) { +static void tesla_init(int16_t param) +{ controls_allowed = 0; tesla_ignition_started = 0; gmlan_switch_init(1); //init the gmlan switch with 1s timeout enabled } -static int tesla_ign_hook() { +static int tesla_ign_hook() +{ return tesla_ignition_started; } -static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { - +static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) +{ + int32_t addr = to_fwd->RIR >> 21; - - if (bus_num == 0) { - + + if (bus_num == 0) + { + // change inhibit of GTW_epasControl - if (addr == 0x101) { + if (addr == 0x101) + { to_fwd->RDLR = to_fwd->RDLR | 0x4000; // 0x4000: WITH_ANGLE, 0xC000: WITH_BOTH (angle and torque) int checksum = (((to_fwd->RDLR & 0xFF00) >> 8) + (to_fwd->RDLR & 0xFF) + 2) & 0xFF; to_fwd->RDLR = to_fwd->RDLR & 0xFFFF; @@ -262,14 +318,22 @@ static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { return 2; } + // remove EPB_epasControl + if (addr == 0x214) + { + return false; + } + return 2; // Custom EPAS bus } - if (bus_num == 2) { - + if (bus_num == 2) + { + // remove GTW_epasControl in forwards - if (addr == 0x101) { - return false; - } + if (addr == 0x101) + { + return false; + } return 0; // Chassis CAN } @@ -277,10 +341,10 @@ static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { } const safety_hooks tesla_hooks = { - .init = tesla_init, - .rx = tesla_rx_hook, - .tx = tesla_tx_hook, - .tx_lin = tesla_tx_lin_hook, - .ignition = tesla_ign_hook, - .fwd = tesla_fwd_hook, + .init = tesla_init, + .rx = tesla_rx_hook, + .tx = tesla_tx_hook, + .tx_lin = tesla_tx_lin_hook, + .ignition = tesla_ign_hook, + .fwd = tesla_fwd_hook, }; diff --git a/panda/board/tools/dfu-util-aarch64 b/panda/board/tools/dfu-util-aarch64 old mode 100755 new mode 100644 diff --git a/panda/board/tools/dfu-util-aarch64-linux b/panda/board/tools/dfu-util-aarch64-linux old mode 100755 new mode 100644 diff --git a/panda/board/tools/dfu-util-x86_64-linux b/panda/board/tools/dfu-util-x86_64-linux old mode 100755 new mode 100644 diff --git a/panda/board/tools/enter_download_mode.py b/panda/board/tools/enter_download_mode.py old mode 100755 new mode 100644 diff --git a/panda/boardesp/get_sdk.sh b/panda/boardesp/get_sdk.sh old mode 100755 new mode 100644 diff --git a/panda/boardesp/get_sdk_mac.sh b/panda/boardesp/get_sdk_mac.sh old mode 100755 new mode 100644 diff --git a/panda/drivers/linux/test/run.sh b/panda/drivers/linux/test/run.sh old mode 100755 new mode 100644 diff --git a/panda/examples/can_bit_transition.py b/panda/examples/can_bit_transition.py old mode 100755 new mode 100644 diff --git a/panda/examples/can_logger.py b/panda/examples/can_logger.py old mode 100755 new mode 100644 diff --git a/panda/examples/can_unique.py b/panda/examples/can_unique.py old mode 100755 new mode 100644 diff --git a/panda/examples/query_vin_and_stats.py b/panda/examples/query_vin_and_stats.py old mode 100755 new mode 100644 diff --git a/panda/python/esptool.py b/panda/python/esptool.py old mode 100755 new mode 100644 diff --git a/panda/python/flash_release.py b/panda/python/flash_release.py old mode 100755 new mode 100644 diff --git a/panda/python/update.py b/panda/python/update.py old mode 100755 new mode 100644 diff --git a/panda/release/make_release.sh b/panda/release/make_release.sh old mode 100755 new mode 100644 diff --git a/panda/release/ota_release.sh b/panda/release/ota_release.sh old mode 100755 new mode 100644 diff --git a/panda/run_automated_tests.sh b/panda/run_automated_tests.sh old mode 100755 new mode 100644 diff --git a/panda/tests/all_wifi_test.py b/panda/tests/all_wifi_test.py old mode 100755 new mode 100644 diff --git a/panda/tests/can_printer.py b/panda/tests/can_printer.py old mode 100755 new mode 100644 diff --git a/panda/tests/debug_console.py b/panda/tests/debug_console.py old mode 100755 new mode 100644 diff --git a/panda/tests/disable_esp.py b/panda/tests/disable_esp.py old mode 100755 new mode 100644 diff --git a/panda/tests/elm_car_simulator.py b/panda/tests/elm_car_simulator.py old mode 100755 new mode 100644 diff --git a/panda/tests/elm_throughput.py b/panda/tests/elm_throughput.py old mode 100755 new mode 100644 diff --git a/panda/tests/flashing_loop.sh b/panda/tests/flashing_loop.sh old mode 100755 new mode 100644 diff --git a/panda/tests/get_version.py b/panda/tests/get_version.py old mode 100755 new mode 100644 diff --git a/panda/tests/gmbitbang/recv.py b/panda/tests/gmbitbang/recv.py old mode 100755 new mode 100644 diff --git a/panda/tests/gmbitbang/rigol.py b/panda/tests/gmbitbang/rigol.py old mode 100755 new mode 100644 diff --git a/panda/tests/gmbitbang/test.py b/panda/tests/gmbitbang/test.py old mode 100755 new mode 100644 diff --git a/panda/tests/gmbitbang/test_one.py b/panda/tests/gmbitbang/test_one.py old mode 100755 new mode 100644 diff --git a/panda/tests/location_listener.py b/panda/tests/location_listener.py old mode 100755 new mode 100644 diff --git a/panda/tests/loopback_test.py b/panda/tests/loopback_test.py old mode 100755 new mode 100644 diff --git a/panda/tests/pedal/enter_canloader.py b/panda/tests/pedal/enter_canloader.py old mode 100755 new mode 100644 diff --git a/panda/tests/read_st_flash.sh b/panda/tests/read_st_flash.sh old mode 100755 new mode 100644 diff --git a/panda/tests/safety/test.sh b/panda/tests/safety/test.sh old mode 100755 new mode 100644 diff --git a/panda/tests/safety/test_honda.py b/panda/tests/safety/test_honda.py old mode 100755 new mode 100644 diff --git a/panda/tests/standalone_test.py b/panda/tests/standalone_test.py old mode 100755 new mode 100644 diff --git a/panda/tests/throughput_test.py b/panda/tests/throughput_test.py old mode 100755 new mode 100644 diff --git a/panda/tests/tucan_loopback.py b/panda/tests/tucan_loopback.py old mode 100755 new mode 100644 diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py new file mode 100644 index 00000000000000..88cc6f2ad94577 --- /dev/null +++ b/selfdrive/car/tesla/ACC_module.py @@ -0,0 +1,280 @@ +from selfdrive.services import service_list +from selfdrive.car.tesla.values import AH, CruiseButtons, CAR +from selfdrive.config import Conversions as CV +import selfdrive.messaging as messaging +import custom_alert as customAlert +import os +import subprocess +import time +import zmq + +def _current_time_millis(): + return int(round(time.time() * 1000)) + +class ACCController(object): + def __init__(self,carcontroller): + self.CC = carcontroller + self.human_cruise_action_time = 0 + self.automated_cruise_action_time = 0 + self.last_angle = 0. + context = zmq.Context() + self.poller = zmq.Poller() + self.live20 = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=self.poller) + self.lead_1 = None + self.last_update_time = 0 + self.enable_adaptive_cruise = False + self.last_cruise_stalk_pull_time = 0 + self.prev_steering_wheel_stalk = None + self.prev_cruise_buttons = CruiseButtons.IDLE + self.prev_cruise_setting = CruiseButtons.IDLE + self.acc_speed_kph = 0. + + #function to calculate the desired cruise speed based on a safe follow distance + def calc_follow_speed(self, CS): + follow_time = 2.5 #in seconds + current_time_ms = int(round(time.time() * 1000)) + #make sure we were able to populate lead_1 + if self.lead_1 is None: + return None + #dRel is in meters + lead_dist = self.lead_1.dRel + #grab the relative speed and convert from m/s to kph + rel_speed = self.lead_1.vRel * 3.6 + #current speed in kph + cur_speed = CS.v_ego * 3.6 + #v_ego is in m/s, so safe_distance is in meters + safe_dist = CS.v_ego * follow_time + # How much we can accelerate without exceeding the max allowed speed. + available_speed = self.acc_speed_kph - CS.v_cruise_actual + # Tesla cruise only functions above 18 MPH + min_cruise_speed = 18 * CV.MPH_TO_MS + # Metric cars adjust cruise in units of 1 and 5 kph + half_press_kph = 1 + full_press_kph = 5 + # Imperial unit cars adjust cruise in units of 1 and 5 mph + if CS.imperial_speed_units: + half_press_kph = 1 * CV.MPH_TO_KPH + full_press_kph = 5 * CV.MPH_TO_KPH + #button to issue + button = None + #debug msg + msg = None + + #print "dRel: ", self.lead_1.dRel," yRel: ", self.lead_1.yRel, " vRel: ", self.lead_1.vRel, " aRel: ", self.lead_1.aRel, " vLead: ", self.lead_1.vLead, " vLeadK: ", self.lead_1.vLeadK, " aLeadK: ", self.lead_1.aLeadK + + ### Logic to determine best cruise speed ### + + # Automatically engange traditional cruise if it is idle and we are + # going fast enough. + if (CS.pcm_acc_status == 1 + and self.enable_adaptive_cruise + and CS.v_ego > min_cruise_speed): + button = CruiseButtons.DECEL_SET + # If traditional cruise is engaged, then control it. + elif CS.pcm_acc_status == 2: + #if lead_dist is reported as 0, no one is detected in front of you so you can speed up + #don't speed up when steer-angle > 2; vision radar often loses lead car in a turn + if lead_dist == 0 and self.enable_adaptive_cruise and CS.angle_steers < 2.0: + if full_press_kph < (available_speed * 0.9): + msg = "5 MPH UP full: ","{0:.1f}kph".format(full_press_kph), " avail: {0:.1f}kph".format(available_speed) + button = CruiseButtons.RES_ACCEL_2ND + elif half_press_kph < (available_speed * 0.8): + msg = "1 MPH UP half: ","{0:.1f}kph".format(half_press_kph), " avail: {0:.1f}kph".format(available_speed) + button = CruiseButtons.RES_ACCEL + + #if we have a populated lead_distance + elif (lead_dist > 0 + #and we only issue commands every 300ms + and current_time_ms > self.automated_cruise_action_time + 300): + ### Slowing down ### + #Reduce speed significantly if lead_dist < 50% of safe dist, no matter the rel_speed + if lead_dist < (safe_dist * 0.5): + msg = "50pct down" + button = CruiseButtons.DECEL_2ND + #Reduce speed significantly if lead_dist < 60% of safe dist + #and if the lead car isn't pulling away + elif lead_dist < (safe_dist * 0.7) and rel_speed < 5: + msg = "70pct down" + button = CruiseButtons.DECEL_2ND + #Reduce speed if rel_speed < -15kph so you don't rush up to lead car + elif rel_speed < -15: + msg = "relspd -15 down" + button = CruiseButtons.DECEL_SET + #we're close to the safe distance, so make slow adjustments + #only adjust every 1 secs + elif (lead_dist < (safe_dist * 0.9) and rel_speed < 0 + and current_time_ms > self.automated_cruise_action_time + 1000): + msg = "90pct down" + button = CruiseButtons.DECEL_SET + + ### Speed up ### + #don't speed up again until you have more than a safe distance in front + #only adjust every 2 sec + elif (lead_dist > safe_dist * 1.2 and half_press_kph < available_speed * 0.8 + and current_time_ms > self.automated_cruise_action_time + 2000): + msg = "120pct UP half: ","{0:.1f}kph".format(half_press_kph), " avail: {0:.1f}kph".format(available_speed) + button = CruiseButtons.RES_ACCEL + + #if we don't need to do any of the above, then we're at a pretty good speed + #make sure if we're at this point that the set cruise speed isn't set too low or high + if (cur_speed - CS.v_cruise_actual) > 5 and button == None: + # Send cruise stalk up_1st if the set speed is too low to bring it up + msg = "cruise rectify" + button = CruiseButtons.RES_ACCEL + + + if (current_time_ms > self.last_update_time + 1000): + #print "Lead Dist: ", "{0:.1f}".format(lead_dist*3.28), "ft Safe Dist: ", "{0:.1f}".format(safe_dist*3.28), "ft Rel Speed: ","{0:.1f}".format(rel_speed), "kph SpdOffset: ", "{0:.3f}".format(speed_delta * 1.01) + ratio = 0 + if safe_dist > 0: + ratio = (lead_dist / safe_dist) * 100 + #print "Ratio: {0:.1f}%".format(ratio), " lead: ","{0:.1f}m".format(lead_dist)," avail: ","{0:.1f}kph".format(available_speed), " Rel Speed: ","{0:.1f}kph".format(rel_speed), " Angle: {0:.1f}deg".format(CS.angle_steers) + self.last_update_time = current_time_ms + #if msg != None: + # print msg + + return button + + def update_stat(self,CS, enabled): + # Check if the cruise stalk was double pulled, indicating that adaptive + # cruise control should be enabled. Twice in .75 seconds counts as a double + # pull. + adaptive_cruise_prev = self.enable_adaptive_cruise + curr_time_ms = _current_time_millis() + speed_uom = 1. + if CS.imperial_speed_units: + speed_uom = 1.609 + if (CS.cruise_buttons == CruiseButtons.MAIN and + self.prev_cruise_buttons != CruiseButtons.MAIN): + double_pull = (curr_time_ms - self.last_cruise_stalk_pull_time < 750) and \ + (CS.cstm_btns.get_button_status("acc")>0) and enabled and \ + ((CS.pcm_acc_status == 2) or (CS.pcm_acc_status == 1)) + if(not self.enable_adaptive_cruise) and double_pull: + customAlert.custom_alert_message("ACC Enabled",CS,150) + self.enable_adaptive_cruise = True + CS.cstm_btns.set_button_status("acc",2) + self.acc_speed_kph = CS.v_ego_raw * 3.6 + elif self.enable_adaptive_cruise and double_pull: + #already enabled, reset speed to current speed + customAlert.custom_alert_message("ACC Speed Updated",CS,150) + self.acc_speed_kph = CS.v_ego_raw * 3.6 + elif self.enable_adaptive_cruise and not double_pull: + customAlert.custom_alert_message("ACC Disabled",CS,150) + CS.cstm_btns.set_button_status("acc",1) + self.enable_adaptive_cruise = False + self.last_cruise_stalk_pull_time = curr_time_ms + elif (CS.cruise_buttons == CruiseButtons.CANCEL and + self.prev_cruise_buttons != CruiseButtons.CANCEL): + self.enable_adaptive_cruise = False + if adaptive_cruise_prev == True: + customAlert.custom_alert_message("ACC Disabled",CS,150) + CS.cstm_btns.set_button_status("acc",1) + self.last_cruise_stalk_pull_time = 0 + elif (self.enable_adaptive_cruise and CS.cruise_buttons !=self.prev_cruise_buttons): + #enabled and new stalk command, let's see what we do with speed + if CS.cruise_buttons == CruiseButtons.RES_ACCEL: + self.acc_speed_kph += speed_uom + if self.acc_speed_kph > 170: + self.acc_speed_kph = 170 + if CS.cruise_buttons == CruiseButtons.RES_ACCEL_2ND: + self.acc_speed_kph += 5 * speed_uom + if self.acc_speed_kph > 170: + self.acc_speed_kph = 170 + if CS.cruise_buttons == CruiseButtons.DECEL_SET: + self.acc_speed_kph -= speed_uom + if self.acc_speed_kph < 0: + self.acc_speed_kph = 0 + if CS.cruise_buttons == CruiseButtons.DECEL_2ND: + self.acc_speed_kph -= 5 * speed_uom + if self.acc_speed_kph < 0: + self.acc_speed_kph = 0 + #if ACC was on and something disabled cruise control, disable ACC too + elif (self.enable_adaptive_cruise == True and + CS.pcm_acc_status != 2 and + curr_time_ms - self.last_cruise_stalk_pull_time > 2000): + self.enable_adaptive_cruise = False + customAlert.custom_alert_message("ACC Disabled",CS,150) + CS.cstm_btns.set_button_status("acc",1) + self.prev_steering_wheel_stalk = CS.steering_wheel_stalk + self.prev_cruise_buttons = CS.cruise_buttons + #now let's see if the ACC is available + if (CS.cstm_btns.get_button_status("acc")==1) or (CS.cstm_btns.get_button_status("acc")==9): + if enabled and ((CS.pcm_acc_status == 2) or (CS.pcm_acc_status == 1)): + CS.cstm_btns.set_button_status("acc",1) + else: + CS.cstm_btns.set_button_status("acc",9) + + + + def update_acc(self,enabled,CS,frame,actuators,pcm_speed): + # Adaptive cruise control + #Bring in the lead car distance from the Live20 feed + l20 = None + if enabled: + for socket, event in self.poller.poll(0): + if socket is self.live20: + l20 = messaging.recv_one(socket) + if l20 is not None: + self.lead_1 = l20.live20.leadOne + + current_time_ms = int(round(time.time() * 1000)) + if CS.cruise_buttons not in [CruiseButtons.IDLE, CruiseButtons.MAIN]: + self.human_cruise_action_time = current_time_ms + button_to_press = None + # The difference between OP's target speed and the current cruise + # control speed, in KPH. + speed_offset = (pcm_speed * CV.MS_TO_KPH - CS.v_cruise_actual) + # Tesla cruise only functions above 18 MPH + min_cruise_speed = 18 * CV.MPH_TO_MS + + if (self.enable_adaptive_cruise + # Only do ACC if OP is steering + and enabled + # And adjust infrequently, since sending repeated adjustments makes + # the car think we're doing a 'long press' on the cruise stalk, + # resulting in small, jerky speed adjustments. + and current_time_ms > self.automated_cruise_action_time + 1000): + # Automatically engange traditional cruise if it is idle and we are + # going fast enough and we are accelerating. + if (CS.pcm_acc_status == 1 + and CS.v_ego > min_cruise_speed + and CS.a_ego > 0.1): + button_to_press = CruiseButtons.DECEL_2ND + # If traditional cruise is engaged, then control it. + elif (CS.pcm_acc_status == 2 + # But don't make adjustments if a human has manually done so in + # the last 3 seconds. Human intention should not be overridden. + and current_time_ms > self.human_cruise_action_time + 3000): + + if CS.imperial_speed_units: + # Imperial unit cars adjust cruise in units of 1 and 5 mph. + half_press_kph = 1 * CV.MPH_TO_KPH + full_press_kph = 5 * CV.MPH_TO_KPH + else: + # Metric cars adjust cruise in units of 1 and 5 kph. + half_press_kph = 1 + full_press_kph = 5 + + # Reduce cruise speed significantly if necessary. + if speed_offset < (-1 * full_press_kph): + # Send cruise stalk dn_2nd. + button_to_press = CruiseButtons.DECEL_2ND + # Reduce speed slightly if necessary. + elif speed_offset < (-1 * half_press_kph): + # Send cruise stalk dn_1st. + button_to_press = CruiseButtons.DECEL_SET + # Increase cruise speed if possible. + elif CS.v_ego > min_cruise_speed: + # How much we can accelerate without exceeding max allowed speed. + available_speed = self.acc_speed_kph - CS.v_cruise_actual + if speed_offset > full_press_kph and speed_offset < available_speed: + # Send cruise stalk up_2nd. + button_to_press = CruiseButtons.RES_ACCEL_2ND + elif speed_offset > half_press_kph and speed_offset < available_speed: + # Send cruise stalk up_1st. + button_to_press = CruiseButtons.RES_ACCEL + if CS.cstm_btns.btns[1].btn_label2 == "Mod JJ": + button_to_press = self.calc_follow_speed(CS) + if button_to_press: + self.automated_cruise_action_time = current_time_ms + return button_to_press diff --git a/selfdrive/car/tesla/ALCA_module.py b/selfdrive/car/tesla/ALCA_module.py new file mode 100644 index 00000000000000..722a0f49c49914 --- /dev/null +++ b/selfdrive/car/tesla/ALCA_module.py @@ -0,0 +1,268 @@ +import custom_alert as tcm +from common.numpy_fast import interp + + +#change lane delta angles and other params +CL_MAXD_BP = [1., 32, 44.] +CL_MAXD_A = [.115, 0.081, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75 +#0.477611 [ +CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s +CL_MAX_A = 10. # do not turn if actuator wants more than x deg for going straight; this should be interp based on speed + + +class ALCAController(object): + def __init__(self,carcontroller,alcaEnabled,steerByAngle): + self.CC = carcontroller #added to start, will see if we need it actually + #variables for lane change + self.alcaEnabled = alcaEnabled + self.laneChange_steerByAngle = steerByAngle + self.laneChange_last_actuator_angle = 0. + self.laneChange_last_actuator_delta = 0. + self.laneChange_last_sent_angle = 0. + self.laneChange_over_the_line = 0 #did we cross the line? + self.laneChange_avg_angle = 0. #used if we do average entry angle over x frames + self.laneChange_avg_count = 0. #used if we do average entry angle over x frames + self.laneChange_enabled = 1 #set to zero for no lane change + self.laneChange_counter = 0 #used to count frames during lane change + self.laneChange_min_duration = 2. #min time to wait before looking for next lane + self.laneChange_duration = 5.6 #how many max seconds to actually do the move; if lane not found after this then send error + self.laneChange_wait = 2 #how many seconds to wait before it starts the change + self.laneChange_lw = 3.7 #lane width in meters + self.laneChange_angle = 0. #saves the last angle from actuators before lane change starts + self.laneChange_angled = 0. #angle delta + self.laneChange_steerr = 16.75 #steer ratio for lane change + self.laneChange_direction = 0 #direction of the lane change + self.prev_right_blinker_on = False #local variable for prev position + self.prev_left_blinker_on = False #local variable for prev position + + def update_status(self,alcaEnabled): + self.alcaEnabled = alcaEnabled + + def update_angle(self,enabled,CS,frame,actuators): + # Basic highway lane change logic + changing_lanes = CS.right_blinker_on or CS.left_blinker_on + + actuator_delta = 0. + laneChange_angle = 0. + turn_signal_needed = 0 #send 1 for left, 2 for right 0 for not needed + + if (not CS.right_blinker_on) and (not CS.left_blinker_on) and \ + (self.laneChange_enabled ==7): + self.laneChange_enabled =1 + self.laneChange_counter =0 + self.laneChange_direction =0 + tcm.custom_alert_message("",CS,0) + + if (not CS.right_blinker_on) and (not CS.left_blinker_on) and \ + (self.laneChange_enabled > 1): + #no blinkers on but we are still changing lane, so we need to send blinker command + if self.laneChange_direction == -1: + turn_signal_needed = 1 + elif self.laneChange_direction == 1: + turn_signal_needed = 2 + else: + turn_signal_needed = 0 + + + if self.alcaEnabled and (self.laneChange_enabled == 1): + if ((CS.v_ego < CL_MIN_V) or (abs(actuators.steerAngle) >= CL_MAX_A) or \ + (abs(CS.angle_steers)>= CL_MAX_A) or (not enabled)): + CS.cstm_btns.set_button_status("alca",9) + else: + CS.cstm_btns.set_button_status("alca",1) + + if self.alcaEnabled and enabled and (((not self.prev_right_blinker_on) and CS.right_blinker_on) or \ + ((not self.prev_left_blinker_on) and CS.left_blinker_on)) and \ + ((CS.v_ego < CL_MIN_V) or (abs(actuators.steerAngle) >= CL_MAX_A) or (abs(CS.angle_steers) >=CL_MAX_A)): + #something is not right, the speed or angle is limitting + tcm.custom_alert_message("Auto Lane Change Unavailable!",CS,500) + CS.cstm_btns.set_button_status("alca",9) + + + if self.alcaEnabled and enabled and (((not self.prev_right_blinker_on) and CS.right_blinker_on) or \ + ((not self.prev_left_blinker_on) and CS.left_blinker_on)) and \ + (CS.v_ego >= CL_MIN_V) and (abs(actuators.steerAngle) < CL_MAX_A): + # start blinker, speed and angle is within limits, let's go + laneChange_direction = 1 + #changing lanes + if CS.left_blinker_on: + laneChange_direction = -1 + if (self.laneChange_enabled > 1) and (self.laneChange_direction <> laneChange_direction): + #something is not right; signal in oposite direction; cancel + tcm.custom_alert_message("Auto Lane Change Canceled! (s)",CS,200) + self.laneChange_enabled = 1 + self.laneChange_counter = 0 + self.laneChange_direction = 0 + CS.cstm_btns.set_button_status("alca",1) + elif (self.laneChange_enabled == 1) : + #compute angle delta for lane change + tcm.custom_alert_message("Auto Lane Change Engaged! (1)",CS,100) + self.laneChange_enabled = 5 + self.laneChange_counter = 1 + self.laneChange_direction = laneChange_direction + self.laneChange_avg_angle = 0. + self.laneChange_avg_count = 0. + self.laneChange_angled = self.laneChange_direction * self.laneChange_steerr * interp(CS.v_ego, CL_MAXD_BP, CL_MAXD_A) + self.laneChange_last_actuator_angle = 0. + self.laneChange_last_actuator_delta = 0. + self.laneChange_over_the_line = 0 + CS.cstm_btns.set_button_status("alca",2) + + if (not self.alcaEnabled) and self.laneChange_enabled > 1: + self.laneChange_enabled = 1 + self.laneChange_counter = 0 + self.laneChange_direction = 0 + + #lane change in process + if self.laneChange_enabled > 1: + if (CS.steer_override or (CS.v_ego < CL_MIN_V)): + tcm.custom_alert_message("Auto Lane Change Canceled! (u)",CS,200) + #if any steer override cancel process or if speed less than min speed + self.laneChange_counter = 0 + self.laneChange_enabled = 1 + self.laneChange_direction = 0 + CS.cstm_btns.set_button_status("alca",1) + # this is the main stage once we start turning + # we have to detect when to let go control back to OP or raise alarm if max timer passed + # there are three conditions we look for: + # 1. we can detect when we cross the lane, and then we let go control to OP + # 2. we passed the min timer to cross the line and the delta between actuator and our angle + # is less than the release angle, then we let go control to OP + # 3. the delta between our angle and the actuator is higher than the previous one + # (we cross the optimal path), then we let go control to OP + # 4. max time is achieved: alert and disengage + # CONTROL: during this time we use ALCA angle to steer (ALCA Control) + + # TODO: - add some logic once we cross the line to smooth the transition to OP + # (needs about 0.1-0.2 s at low angles to get to the point where it's smooth) + # - check if the 0.5 * angle_delta is enough of a test for lane change + # - check that the delta in angle from actuator is increasing at a constant rate of DA/x + # or adjust angle for turns in road + # - generate audible alert for errors/take over messages + if self.laneChange_enabled ==3: + if self.laneChange_counter == 1: + tcm.custom_alert_message("Auto Lane Change Engaged! (4)",CS,800) + self.laneChange_over_the_line = 0 + self.laneChange_counter += 1 + laneChange_angle = self.laneChange_angled + if (self.laneChange_over_the_line == 0): + #we didn't cross the line, so keep computing the actuator delta until it flips + actuator_delta = self.laneChange_direction * (-actuators.steerAngle - self.laneChange_last_actuator_angle) + actuator_ratio = (-actuators.steerAngle)/self.laneChange_last_actuator_angle + if (actuator_ratio < 1) and (abs(actuator_delta) > 0.5 * abs(self.laneChange_angled)): + #sudden change in actuator angle or sign means we are on the other side of the line + tcm.custom_alert_message("Auto Lane Change Engaged! (5)",CS,800) + self.laneChange_over_the_line = 1 + if self.laneChange_over_the_line ==1: + self.laneChange_enabled = 7 + self.laneChange_counter = 1 + self.laneChange_direction = 0 + #we are on the other side, let control go to OP + if self.laneChange_counter > (self.laneChange_duration) * 100: + self.laneChange_enabled = 1 + self.laneChange_counter = 0 + tcm.custom_alert_message("Auto Lane Change Canceled! (t)",CS,200) + self.laneChange_counter = 0 + CS.cstm_btns.set_button_status("alca",1) + # this is the critical start of the turn + # here we will detect the angle to move; it is based on a speed determined angle but we have to also + # take in consideration what's happening with the delta of consecutive actuators + # if they go in the same direction with our turn we have to reset our turn angle because the actuator either + # is compensating for a turn in the road OR for same lane correction for the car + # CONTROL: during this time we use ALCA angle to steer (ALCA Control) + + # TODO: - when actuator moves in the same direction with lane change, correct starting angle + if self.laneChange_enabled == 4: + if self.laneChange_counter == 1: + self.laneChange_angle = -actuators.steerAngle + tcm.custom_alert_message("Auto Lane Change Engaged! (3)",CS,100) + self.laneChange_angled = self.laneChange_direction * self.laneChange_steerr * interp(CS.v_ego, CL_MAXD_BP, CL_MAXD_A) + #if angle more than max angle allowed cancel; last chance to cancel on road curvature + if (abs(self.laneChange_angle) > CL_MAX_A): + tcm.custom_alert_message("Auto Lane Change Canceled! (a)",CS,200) + self.laneChange_enabled = 1 + self.laneChange_counter = 0 + self.laneChange_direction = 0 + CS.cstm_btns.set_button_status("alca",1) + laneChange_angle = self.laneChange_angled * self.laneChange_counter / 50 + self.laneChange_counter += 1 + delta_change = abs(self.laneChange_angle+ laneChange_angle + actuators.steerAngle) - abs(self.laneChange_angled) + if (self.laneChange_counter == 100) or (delta_change >= 0): + if (delta_change < 0): + #didn't achieve desired angle yet, so repeat + self.laneChange_counter = 1 + else: + self.laneChange_enabled = 3 + self.laneChange_counter = 1 + self.laneChange_angled = laneChange_angle + # this is the first stage of the ALCAS + # here we wait for x seconds before we start the turn + # if at any point we detect hand on wheel, we let go of control and notify user + # the same test for hand on wheel is done at ANY stage throughout the lane change process + # CONTROL: during this stage we use the actuator angle to steer (OP Control) + if self.laneChange_enabled == 5: + if self.laneChange_counter == 1: + tcm.custom_alert_message("Auto Lane Change Engaged! (2)",CS,self.laneChange_wait * 100) + self.laneChange_counter += 1 + if self.laneChange_counter > (self.laneChange_wait -1) *100: + self.laneChange_avg_angle += -actuators.steerAngle + self.laneChange_avg_count += 1 + if self.laneChange_counter == self.laneChange_wait * 100: + self.laneChange_enabled = 4 + self.laneChange_counter = 1 + # this is the final stage of the ALCAS + # this just shows a message that we completed the lane change + # CONTROL: during this time we use the actuator angle to steer (OP Control) + if self.laneChange_enabled == 7: + if self.laneChange_counter ==1: + tcm.custom_alert_message("Auto Lane Change Complete!",CS,300) + CS.cstm_btns.set_button_status("alca",1) + self.laneChange_counter +=1 + alca_enabled = (self.laneChange_enabled > 1) + apply_angle = 0. + # save position of blinker stalk + self.prev_right_blinker_on = CS.right_blinker_on + self.prev_left_blinker_on = CS.left_blinker_on + # Angle if 0 we need to save it as a very small nonzero with the same sign as the prev one + self.laneChange_last_actuator_delta = -actuators.steerAngle - self.laneChange_last_actuator_angle + last_angle_sign = 1 + if (self.laneChange_last_actuator_angle <>0): + last_angle_sign = self.laneChange_last_actuator_angle / abs(self.laneChange_last_actuator_angle) + if actuators.steerAngle == 0: + self.laneChange_last_actuator_angle = last_angle_sign * 0.00001 + else: + self.laneChange_last_actuator_angle = -actuators.steerAngle + #determine what angle to send and send it + if (self.laneChange_enabled > 1) and (self.laneChange_enabled < 5): + apply_angle = self.laneChange_angle + laneChange_angle + else: + apply_angle = -actuators.steerAngle + self.laneChange_last_sent_angle = apply_angle + return [-apply_angle,alca_enabled,turn_signal_needed] + + + + def update(self,enabled,CS,frame,actuators): + if self.alcaEnabled: + # ALCA enabled + if self.laneChange_steerByAngle: + # steering by angle + new_angle = 0. + new_ALCA_enabled = False + new_turn_signal = 0 + new_angle,new_ALCA_Enabled,new_turn_signal = self.update_angle(enabled,CS,frame,actuators) + return [new_angle,new_ALCA_Enabled,new_turn_signal] + else: + # steering by torque + #TODO: torque ALCA module + return [actuators.steerAngle,False,0] + else: + # ALCA disabled + if self.laneChange_steerByAngle: + #steer by angle + return [actuators.steerAngle,False,0] + else: + #steer by torque + #TODO: torque ALCA module + return [actuators.steerAngle,False,0] + diff --git a/selfdrive/car/tesla/HSO_module.py b/selfdrive/car/tesla/HSO_module.py new file mode 100644 index 00000000000000..35d3ebc677681f --- /dev/null +++ b/selfdrive/car/tesla/HSO_module.py @@ -0,0 +1,43 @@ +#human steer override module +from selfdrive.config import Conversions as CV +import selfdrive.messaging as messaging +import custom_alert as customAlert +import time + +def _current_time_millis(): + return int(round(time.time() * 1000)) + +class HSOController(object): + def __init__(self,carcontroller): + self.CC = carcontroller + self.frame_humanSteered = 0 + + + def update_stat(self,CS,enabled,actuators,frame): + human_control = False + if (CS.cstm_btns.get_button_status("steer") >0) and enabled: + #if steering but not by ALCA + if (CS.right_blinker_on or CS.left_blinker_on) and (self.CC.ALCA.laneChange_enabled <= 1): + self.frame_humanSteered = frame + if (CS.steer_override > 0): + self.frame_humanSteered = frame + else: + if (frame - self.frame_humanSteered < 50): # Need more human testing of handoff timing + # Find steering difference between visiond model and human (no need to do every frame if we run out of CPU): + steer_current=(CS.angle_steers) # Formula to convert current steering angle to match apply_steer calculated number + apply_steer = int(-actuators.steerAngle) + angle = abs(apply_steer-steer_current) + if angle > 5.: + self.frame_humanSteered = frame + if enabled: + if CS.cstm_btns.get_button_status("steer") > 0: + if (frame - self.frame_humanSteered < 50): + human_control = True + CS.cstm_btns.set_button_status("steer",3) + customAlert.custom_alert_message("Manual Steering Enabled",CS,51) + else: + CS.cstm_btns.set_button_status("steer",2) + else: + if CS.cstm_btns.get_button_status("steer") > 0: + CS.cstm_btns.set_button_status("steer",1) + return human_control and enabled diff --git a/selfdrive/car/tesla/PCC_module.py b/selfdrive/car/tesla/PCC_module.py new file mode 100644 index 00000000000000..6f632b6a893679 --- /dev/null +++ b/selfdrive/car/tesla/PCC_module.py @@ -0,0 +1,291 @@ +from selfdrive.services import service_list +from selfdrive.car.tesla.values import AH, CruiseButtons, CAR +from selfdrive.config import Conversions as CV +import selfdrive.messaging as messaging +import custom_alert as customAlert +import os +import subprocess +import time +import zmq + +def _current_time_millis(): + return int(round(time.time() * 1000)) + +#this is for the pedal cruise control +class PCCController(object): + def __init__(self,carcontroller): + self.CC = carcontroller + self.human_cruise_action_time = 0 + self.automated_cruise_action_time = 0 + self.last_angle = 0. + context = zmq.Context() + self.poller = zmq.Poller() + self.live20 = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=self.poller) + self.lead_1 = None + self.last_update_time = 0 + self.enable_pedal_cruise = False + self.last_cruise_stalk_pull_time = 0 + self.prev_steering_wheel_stalk = None + self.prev_cruise_buttons = CruiseButtons.IDLE + self.prev_cruise_setting = CruiseButtons.IDLE + self.pedal_hardware_present = True + self.pedal_speed_kph = 0. + + #function to calculate the desired cruise speed based on a safe follow distance + """ from ACC, leave here until we implement + def calc_follow_speed(self, CS): + follow_time = 2.5 #in seconds + current_time_ms = int(round(time.time() * 1000)) + #make sure we were able to populate lead_1 + if self.lead_1 is None: + return None + #dRel is in meters + lead_dist = self.lead_1.dRel + #grab the relative speed and convert from m/s to kph + rel_speed = self.lead_1.vRel * 3.6 + #current speed in kph + cur_speed = CS.v_ego * 3.6 + #v_ego is in m/s, so safe_distance is in meters + safe_dist = CS.v_ego * follow_time + # How much we can accelerate without exceeding the max allowed speed. + available_speed = CS.v_cruise_pcm - CS.v_cruise_actual + # Tesla cruise only functions above 18 MPH + min_cruise_speed = 18 * CV.MPH_TO_MS + # Metric cars adjust cruise in units of 1 and 5 kph + half_press_kph = 1 + full_press_kph = 5 + # Imperial unit cars adjust cruise in units of 1 and 5 mph + if CS.imperial_speed_units: + half_press_kph = 1 * CV.MPH_TO_KPH + full_press_kph = 5 * CV.MPH_TO_KPH + #button to issue + button = None + #debug msg + msg = None + + #print "dRel: ", self.lead_1.dRel," yRel: ", self.lead_1.yRel, " vRel: ", self.lead_1.vRel, " aRel: ", self.lead_1.aRel, " vLead: ", self.lead_1.vLead, " vLeadK: ", self.lead_1.vLeadK, " aLeadK: ", self.lead_1.aLeadK + + ### Logic to determine best cruise speed ### + + # Automatically engange traditional cruise if it is idle and we are + # going fast enough. + if (CS.pcm_acc_status == 1 + and self.enable_pedal_cruise + and CS.v_ego > min_cruise_speed): + button = CruiseButtons.DECEL_SET + # If traditional cruise is engaged, then control it. + elif CS.pcm_acc_status == 2: + #if lead_dist is reported as 0, no one is detected in front of you so you can speed up + #don't speed up when steer-angle > 2; vision radar often loses lead car in a turn + if lead_dist == 0 and self.enable_pedal_cruise and CS.angle_steers < 2.0: + if full_press_kph < (available_speed * 0.9): + msg = "5 MPH UP full: ","{0:.1f}kph".format(full_press_kph), " avail: {0:.1f}kph".format(available_speed) + button = CruiseButtons.RES_ACCEL_2ND + elif half_press_kph < (available_speed * 0.8): + msg = "1 MPH UP half: ","{0:.1f}kph".format(half_press_kph), " avail: {0:.1f}kph".format(available_speed) + button = CruiseButtons.RES_ACCEL + + #if we have a populated lead_distance + elif (lead_dist > 0 + #and we only issue commands every 300ms + and current_time_ms > self.automated_cruise_action_time + 300): + ### Slowing down ### + #Reduce speed significantly if lead_dist < 50% of safe dist, no matter the rel_speed + if lead_dist < (safe_dist * 0.5): + msg = "50pct down" + button = CruiseButtons.DECEL_2ND + #Reduce speed significantly if lead_dist < 60% of safe dist + #and if the lead car isn't pulling away + elif lead_dist < (safe_dist * 0.7) and rel_speed < 5: + msg = "70pct down" + button = CruiseButtons.DECEL_2ND + #Reduce speed if rel_speed < -15kph so you don't rush up to lead car + elif rel_speed < -15: + msg = "relspd -15 down" + button = CruiseButtons.DECEL_SET + #we're close to the safe distance, so make slow adjustments + #only adjust every 1 secs + elif (lead_dist < (safe_dist * 0.9) and rel_speed < 0 + and current_time_ms > self.automated_cruise_action_time + 1000): + msg = "90pct down" + button = CruiseButtons.DECEL_SET + + ### Speed up ### + #don't speed up again until you have more than a safe distance in front + #only adjust every 2 sec + elif (lead_dist > safe_dist * 1.2 and half_press_kph < available_speed * 0.8 + and current_time_ms > self.automated_cruise_action_time + 2000): + msg = "120pct UP half: ","{0:.1f}kph".format(half_press_kph), " avail: {0:.1f}kph".format(available_speed) + button = CruiseButtons.RES_ACCEL + + #if we don't need to do any of the above, then we're at a pretty good speed + #make sure if we're at this point that the set cruise speed isn't set too low or high + if (cur_speed - CS.v_cruise_actual) > 5 and button == None: + # Send cruise stalk up_1st if the set speed is too low to bring it up + msg = "cruise rectify" + button = CruiseButtons.RES_ACCEL + + + if (current_time_ms > self.last_update_time + 1000): + #print "Lead Dist: ", "{0:.1f}".format(lead_dist*3.28), "ft Safe Dist: ", "{0:.1f}".format(safe_dist*3.28), "ft Rel Speed: ","{0:.1f}".format(rel_speed), "kph SpdOffset: ", "{0:.3f}".format(speed_delta * 1.01) + ratio = 0 + if safe_dist > 0: + ratio = (lead_dist / safe_dist) * 100 + print "Ratio: {0:.1f}%".format(ratio), " lead: ","{0:.1f}m".format(lead_dist)," avail: ","{0:.1f}kph".format(available_speed), " Rel Speed: ","{0:.1f}kph".format(rel_speed), " Angle: {0:.1f}deg".format(CS.angle_steers) + self.last_update_time = current_time_ms + if msg != None: + print msg + + return button""" + + def update_stat(self,CS, enabled): + # Check if the cruise stalk was double pulled, indicating that adaptive + # cruise control should be enabled. Twice in .75 seconds counts as a double + # pull. + curr_time_ms = _current_time_millis() + adaptive_cruise_prev = self.enable_pedal_cruise + speed_uom = 1. + if CS.imperial_speed_units: + speed_uom = 1.609 + if (CS.cruise_buttons == CruiseButtons.MAIN and + self.prev_cruise_buttons != CruiseButtons.MAIN): + double_pull = (curr_time_ms - self.last_cruise_stalk_pull_time < 750) and \ + (CS.cstm_btns.get_button_status("pedal")>0) and enabled and \ + (CS.pcm_acc_status == 0) and self.pedal_hardware_present + if(not self.enable_pedal_cruise) and double_pull: + self.enable_pedal_cruise = True + customAlert.custom_alert_message("PDL Enabled",CS,150) + CS.cstm_btns.set_button_status("pedal",2) + self.pedal_speed_kph = CS.v_ego_raw * 3.6 + elif (self.enable_pedal_cruise) and double_pull: + #already enabled, reset speed to current speed + customAlert.custom_alert_message("PDL Speed Updated",CS,150) + self.pedal_speed_kph = CS.v_ego_raw * 3.6 + elif self.enable_pedal_cruise and not double_pull: + self.enable_pedal_cruise = False + customAlert.custom_alert_message("PDL Disabled",CS,150) + CS.cstm_btns.set_button_status("pedal",1) + self.last_cruise_stalk_pull_time = curr_time_ms + elif (CS.cruise_buttons == CruiseButtons.CANCEL and + self.prev_cruise_buttons != CruiseButtons.CANCEL): + self.enable_pedal_cruise = False + if adaptive_cruise_prev == True: + customAlert.custom_alert_message("PDL Disabled",CS,150) + CS.cstm_btns.set_button_status("pedal",1) + self.last_cruise_stalk_pull_time = 0 + elif (self.enable_pedal_cruise and CS.cruise_buttons !=self.prev_cruise_buttons): + #enabled and new stalk command, let's see what we do with speed + if CS.cruise_buttons == CruiseButtons.RES_ACCEL: + self.pedal_speed_kph += speed_uom + if self.pedal_speed_kph > 170: + self.pedal_speed_kph = 170 + if CS.cruise_buttons == CruiseButtons.RES_ACCEL_2ND: + self.pedal_speed_kph += 5 * speed_uom + if self.pedal_speed_kph > 170: + self.pedal_speed_kph = 170 + if CS.cruise_buttons == CruiseButtons.DECEL_SET: + self.pedal_speed_kph -= speed_uom + if self.pedal_speed_kph < 0: + self.pedal_speed_kph = 0 + if CS.cruise_buttons == CruiseButtons.DECEL_2ND: + self.pedal_speed_kph -= 5 * speed_uom + if self.pedal_speed_kph < 0: + self.pedal_speed_kph = 0 + #if PDL was on and something disabled cruise control, disable PDL too + elif (self.enable_pedal_cruise == True and + CS.pcm_acc_status != 0 and + curr_time_ms - self.last_cruise_stalk_pull_time > 2000): + self.enable_pedal_cruise = False + customAlert.custom_alert_message("PCC Disabled",CS,150) + CS.cstm_btns.set_button_status("pedal",1) + self.prev_steering_wheel_stalk = CS.steering_wheel_stalk + self.prev_cruise_buttons = CS.cruise_buttons + #now let's see if the PDL is available + if (CS.cstm_btns.get_button_status("pedal")==1) or (CS.cstm_btns.get_button_status("pedal")==9): + if enabled and (CS.pcm_acc_status == 0): + CS.cstm_btns.set_button_status("pedal",1) + else: + CS.cstm_btns.set_button_status("pedal",9) + if (CS.cstm_btns.get_button_status("pedal")>0) and not self.pedal_hardware_present: + #no pedal hardware, disable button + CS.cstm_btns.set_button_status("pedal",0) + + def update_pdl(self,enabled,CS,frame,actuators,pcm_speed): + #Pedal cruise control + #if no hardware present, return -1 + if not self.pedal_hardware_present: + return 0,False + + # Adaptive cruise control + #Bring in the lead car distance from the Live20 feed + """ from ACC, leave here until we implement + l20 = None + if enabled: + for socket, event in self.poller.poll(0): + if socket is self.live20: + l20 = messaging.recv_one(socket) + if l20 is not None: + self.lead_1 = l20.live20.leadOne + + current_time_ms = int(round(time.time() * 1000)) + if CS.cruise_buttons not in [CruiseButtons.IDLE, CruiseButtons.MAIN]: + self.human_cruise_action_time = current_time_ms + button_to_press = None + # The difference between OP's target speed and the current cruise + # control speed, in KPH. + speed_offset = (pcm_speed * CV.MS_TO_KPH - CS.v_cruise_actual) + # Tesla cruise only functions above 18 MPH + min_cruise_speed = 18 * CV.MPH_TO_MS + + if (self.enable_pedal_cruise + # Only do ACC if OP is steering + and enabled + # And adjust infrequently, since sending repeated adjustments makes + # the car think we're doing a 'long press' on the cruise stalk, + # resulting in small, jerky speed adjustments. + and current_time_ms > self.automated_cruise_action_time + 1000): + # Automatically engange traditional cruise if it is idle and we are + # going fast enough and we are accelerating. + if (CS.pcm_acc_status == 1 + and CS.v_ego > min_cruise_speed + and CS.a_ego > 0.1): + button_to_press = CruiseButtons.DECEL_2ND + # If traditional cruise is engaged, then control it. + elif (CS.pcm_acc_status == 2 + # But don't make adjustments if a human has manually done so in + # the last 3 seconds. Human intention should not be overridden. + and current_time_ms > self.human_cruise_action_time + 3000): + + if CS.imperial_speed_units: + # Imperial unit cars adjust cruise in units of 1 and 5 mph. + half_press_kph = 1 * CV.MPH_TO_KPH + full_press_kph = 5 * CV.MPH_TO_KPH + else: + # Metric cars adjust cruise in units of 1 and 5 kph. + half_press_kph = 1 + full_press_kph = 5 + + # Reduce cruise speed significantly if necessary. + if speed_offset < (-1 * full_press_kph): + # Send cruise stalk dn_2nd. + button_to_press = CruiseButtons.DECEL_2ND + # Reduce speed slightly if necessary. + elif speed_offset < (-1 * half_press_kph): + # Send cruise stalk dn_1st. + button_to_press = CruiseButtons.DECEL_SET + # Increase cruise speed if possible. + elif CS.v_ego > min_cruise_speed: + # How much we can accelerate without exceeding max allowed speed. + available_speed = CS.v_cruise_pcm - CS.v_cruise_actual + if speed_offset > full_press_kph and speed_offset < available_speed: + # Send cruise stalk up_2nd. + button_to_press = CruiseButtons.RES_ACCEL_2ND + elif speed_offset > half_press_kph and speed_offset < available_speed: + # Send cruise stalk up_1st. + button_to_press = CruiseButtons.RES_ACCEL + if CS.cstm_btns.btns[1].btn_label2 == "Mod JJ": + button_to_press = self.calc_follow_speed(CS) + if button_to_press: + self.automated_cruise_action_time = current_time_ms + return button_to_press""" + return 0,False diff --git a/selfdrive/car/tesla/alert.msg b/selfdrive/car/tesla/alert.msg new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index 785cefeeda3753..3d15346bdffede 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -7,18 +7,25 @@ from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.controls.lib.drive_helpers import rate_limit from common.numpy_fast import clip, interp +import numpy as np +import math as mth +from common.realtime import sec_since_boot from selfdrive.car.tesla import teslacan from selfdrive.car.tesla.values import AH, CruiseButtons, CAR from selfdrive.can.packer import CANPacker from selfdrive.config import Conversions as CV - +import custom_alert as tcm +from ALCA_module import ALCAController +from ACC_module import ACCController +from PCC_module import PCCController +from HSO_module import HSOController # Steer angle limits ANGLE_MAX_BP = [0., 27., 36.] ANGLE_MAX_V = [410., 92., 36.] ANGLE_DELTA_BP = [0., 5., 15.] -ANGLE_DELTA_V = [5., .8, .15] # windup limit -ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit +ANGLE_DELTA_V = [5., .8, .25] # windup limit +ANGLE_DELTA_VU = [5., 3.5, 0.8] # unwind limit def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint): # hyst params... TODO: move these to VehicleParams @@ -73,7 +80,12 @@ def __init__(self, dbc_name, enable_camera=True): self.enable_camera = enable_camera self.packer = CANPacker(dbc_name) self.epas_disabled = True - self.last_angle = 0. + self.last_angle = 0 + self.ALCA = ALCAController(self,True,True) # Enabled and SteerByAngle both True + self.ACC = ACCController(self) + self.PCC = PCCController(self) + self.HSO = HSOController(self) + def update(self, sendcan, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ @@ -137,10 +149,39 @@ def update(self, sendcan, enabled, CS, frame, actuators, \ # Basic highway lane change logic changing_lanes = CS.right_blinker_on or CS.left_blinker_on - enable_steer_control = (enabled and not changing_lanes) + #countdown for custom message timer + tcm.update_custom_alert(CS) + #end countdown for custom + + #update statuses for custom buttons every 0.1 sec + if (frame % 10 == 0): + CS.cstm_btns.read_buttons_in_file() + CS.cstm_btns.write_buttons_out_file() + self.ALCA.update_status(True if CS.cstm_btns.get_button_status("alca") > 0 else False) + #print CS.cstm_btns.get_button_status("alca") + + #update ACC module info + self.ACC.update_stat(CS, True) + #update PCC module info + self.PCC.update_stat(CS, True) + #update HSO module info + human_control = False + + #update CS.v_cruise_pcm based on module selected + if self.ACC.enable_adaptive_cruise: + CS.v_cruise_pcm = self.ACC.acc_speed_kph + elif self.PCC.enable_pedal_cruise: + CS.v_cruise_pcm = self.PCC.pedal_speed_kph + else: + CS.v_cruise_pcm = CS.v_cruise_actual + #get the angle from ALCA + alca_enabled = False + turn_signal_needed = 0 + apply_angle,alca_enabled,turn_signal_needed = self.ALCA.update(enabled,CS,frame,actuators) + apply_angle = -apply_angle #Tesla is reversed vs OP + human_control = self.HSO.update_stat(CS,enabled,actuators,frame) + enable_steer_control = (enabled and ((not changing_lanes) or alca_enabled) and not human_control) - # Angle - apply_angle = -actuators.steerAngle angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V) apply_angle = clip(apply_angle, -angle_lim, angle_lim) # windup slower @@ -150,16 +191,73 @@ def update(self, sendcan, enabled, CS, frame, actuators, \ angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU) apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) - #if blinker is on send the actual angle - if (changing_lanes): + #if human control send the steering angle as read at steering wheel + if human_control: apply_angle = CS.angle_steers + #if blinker is on send the actual angle + #if (changing_lanes and (CS.laneChange_enabled < 2)): + # apply_angle = CS.angle_steers # Send CAN commands. can_sends = [] send_step = 5 if (True): #(frame % send_step) == 0: + """#first we emulate DAS + if (CS.DAS_info_frm == -1): + #initialize all frames + CS.DAS_info_frm = frame # 1.00 s interval + CS.DAS_status_frm = (frame + 10) % 100 # 0.50 s interval + CS.DAS_status2_frm = (frame + 35) % 100 # 0.50 s interval in between DAS_status + CS.DAS_bodyControls_frm = (frame + 40) % 100 # 0.50 s interval + CS.DAS_lanes_frm = (frame + 5) % 100 # 0.10 s interval + CS.DAS_objects_frm = (frame + 2) % 100 # 0.03 s interval + CS.DAS_pscControl_frm = (frame + 3) % 100 # 0.04 s interval + if (CS.DAS_info_frm == frame): + can_sends.append(teslacan.create_DAS_info_msg(CS.DAS_info_msg)) + CS.DAS_info_msg += 1 + CS.DAS_info_msg = CS.DAS_info_msg % 10 + if (CS.DAS_status_frm == frame): + can_sends.append(teslacan.create_DAS_status_msg(CS.DAS_status_idx)) + CS.DAS_status_idx += 1 + CS.DAS_status_idx = CS.DAS_status_idx % 16 + CS.DAS_status_frm = (CS.DAS_status_frm + 50) % 100 + if (CS.DAS_status2_frm == frame): + can_sends.append(teslacan.create_DAS_status2_msg(CS.DAS_status2_idx)) + CS.DAS_status2_idx += 1 + CS.DAS_status2_idx = CS.DAS_status2_idx % 16 + CS.DAS_status2_frm = (CS.DAS_status2_frm + 50) % 100 + if (CS.DAS_bodyControls_frm == frame): + can_sends.append(teslacan.create_DAS_bodyControls_msg(CS.DAS_bodyControls_idx)) + CS.DAS_bodyControls_idx += 1 + CS.DAS_bodyControls_idx = CS.DAS_bodyControls_idx % 16 + CS.DAS_bodyControls_frm = (CS.DAS_bodyControls_frm + 50) % 100 + if (CS.DAS_lanes_frm == frame): + can_sends.append(teslacan.create_DAS_lanes_msg(CS.DAS_lanes_idx)) + CS.DAS_lanes_idx += 1 + CS.DAS_lanes_idx = CS.DAS_lanes_idx % 16 + CS.DAS_lanes_frm = (CS.DAS_lanes_frm + 10) % 100 + if (CS.DAS_pscControl_frm == frame): + can_sends.append(teslacan.create_DAS_pscControl_msg(CS.DAS_pscControl_idx)) + CS.DAS_pscControl_idx += 1 + CS.DAS_pscControl_idx = CS.DAS_pscControl_idx % 16 + CS.DAS_pscControl_frm = (CS.DAS_pscControl_frm + 4) % 100 + if (CS.DAS_objects_frm == frame): + can_sends.append(teslacan.create_DAS_objects_msg(CS.DAS_objects_idx)) + CS.DAS_objects_idx += 1 + CS.DAS_objects_idx = CS.DAS_objects_idx % 16 + CS.DAS_objects_frm = (CS.DAS_objects_frm + 3) % 100 + # end of DAS emulation """ idx = frame % 16 #(frame/send_step) % 16 can_sends.append(teslacan.create_steering_control(enable_steer_control, apply_angle, idx)) can_sends.append(teslacan.create_epb_enable_signal(idx)) + cruise_btn = None + if self.ACC.enable_adaptive_cruise: + cruise_btn = self.ACC.update_acc(enabled,CS,frame,actuators,pcm_speed) + if cruise_btn: + cruise_msg = teslacan.create_cruise_adjust_msg(cruise_btn,-1,CS.steering_wheel_stalk) + can_sends.insert(0, cruise_msg) + elif (turn_signal_needed > 0) and (frame % 2 == 0): + cruise_msg = teslacan.create_cruise_adjust_msg(-1,turn_signal_needed,CS.steering_wheel_stalk) + can_sends.insert(0, cruise_msg) self.last_angle = apply_angle sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 17d05e8b16f074..63a8ed0fc1794e 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -2,10 +2,11 @@ from common.kalman.simple_kalman import KF1D from selfdrive.can.parser import CANParser from selfdrive.config import Conversions as CV -from selfdrive.car.tesla.values import CAR, DBC +from selfdrive.car.tesla.values import CAR, CruiseButtons, DBC +from uibuttons import UIButtons,UIButton import numpy as np from ctypes import create_string_buffer - + def parse_gear_shifter(can_gear_shifter, car_fingerprint): # TODO: Use VAL from DBC to parse this field @@ -23,6 +24,7 @@ def parse_gear_shifter(can_gear_shifter, car_fingerprint): return "unknown" + def calc_cruise_offset(offset, speed): # euristic formula so that speed is controlled to ~ 0.3m/s below pid_speed # constraints to solve for _K0, _K1, _K2 are: @@ -69,15 +71,43 @@ def get_can_signals(CP): ("DI_stateCounter", "DI_state", 0), ("GTW_driverPresent", "GTW_status", 0), ("DI_brakePedal", "DI_torque2", 0), - ("SpdCtrlLvr_Stat", "STW_ACTN_RQ", 0), ("DI_cruiseSet", "DI_state", 0), ("DI_cruiseState", "DI_state", 0), - ("VSL_Enbl_Rq", "STW_ACTN_RQ", 0), # Used in interface.py - ("TSL_RND_Posn_StW","SBW_RQ_SCCM" , 0), ("TSL_P_Psd_StW","SBW_RQ_SCCM" , 0), - ("TurnIndLvr_Stat", "STW_ACTN_RQ", 0), ("DI_motorRPM", "DI_torque1", 0), ("DI_speedUnits", "DI_state", 0), + # Steering wheel stalk signals (useful for managing cruise control) + ("SpdCtrlLvr_Stat", "STW_ACTN_RQ", 0), + ("VSL_Enbl_Rq", "STW_ACTN_RQ", 0), + ("SpdCtrlLvrStat_Inv", "STW_ACTN_RQ", 0), + ("DTR_Dist_Rq", "STW_ACTN_RQ", 0), + ("TurnIndLvr_Stat", "STW_ACTN_RQ", 0), + ("HiBmLvr_Stat", "STW_ACTN_RQ", 0), + ("WprWashSw_Psd", "STW_ACTN_RQ", 0), + ("WprWash_R_Sw_Posn_V2", "STW_ACTN_RQ", 0), + ("StW_Lvr_Stat", "STW_ACTN_RQ", 0), + ("StW_Cond_Flt", "STW_ACTN_RQ", 0), + ("StW_Cond_Psd", "STW_ACTN_RQ", 0), + ("HrnSw_Psd", "STW_ACTN_RQ", 0), + ("StW_Sw00_Psd", "STW_ACTN_RQ", 0), + ("StW_Sw01_Psd", "STW_ACTN_RQ", 0), + ("StW_Sw02_Psd", "STW_ACTN_RQ", 0), + ("StW_Sw03_Psd", "STW_ACTN_RQ", 0), + ("StW_Sw04_Psd", "STW_ACTN_RQ", 0), + ("StW_Sw05_Psd", "STW_ACTN_RQ", 0), + ("StW_Sw06_Psd", "STW_ACTN_RQ", 0), + ("StW_Sw07_Psd", "STW_ACTN_RQ", 0), + ("StW_Sw08_Psd", "STW_ACTN_RQ", 0), + ("StW_Sw09_Psd", "STW_ACTN_RQ", 0), + ("StW_Sw10_Psd", "STW_ACTN_RQ", 0), + ("StW_Sw11_Psd", "STW_ACTN_RQ", 0), + ("StW_Sw12_Psd", "STW_ACTN_RQ", 0), + ("StW_Sw13_Psd", "STW_ACTN_RQ", 0), + ("StW_Sw14_Psd", "STW_ACTN_RQ", 0), + ("StW_Sw15_Psd", "STW_ACTN_RQ", 0), + ("WprSw6Posn", "STW_ACTN_RQ", 0), + ("MC_STW_ACTN_RQ", "STW_ACTN_RQ", 0), + ("CRC_STW_ACTN_RQ", "STW_ACTN_RQ", 0), ] @@ -127,6 +157,8 @@ def get_epas_parser(CP): class CarState(object): def __init__(self, CP): self.brake_only = CP.enableCruise + self.enable_adaptive_cruise = False + self.last_cruise_stalk_pull_time = 0 self.CP = CP self.user_gas, self.user_gas_pressed = 0., 0 @@ -144,6 +176,38 @@ def __init__(self, CP): self.stopped = 0 self.frame_humanSteered = 0 # Last frame human steered + # variables used for the fake DAS creation + self.DAS_info_frm = -1 + self.DAS_info_msg = 0 + self.DAS_status_frm = 0 + self.DAS_status_idx = 0 + self.DAS_status2_frm = 0 + self.DAS_status2_idx = 0 + self.DAS_bodyControls_frm = 0 + self.DAS_bodyControls_idx = 0 + self.DAS_lanes_frm = 0 + self.DAS_lanes_idx = 0 + self.DAS_objects_frm = 0 + self.DAS_objects_idx = 0 + self.DAS_pscControl_frm = 0 + self.DAS_pscControl_idx = 0 + + #variables for pedal CC + self.pedal_speed_kph = 0. + self.pedal_enabled = 0 + + #variable for custom buttons + self.cstm_btns = UIButtons() + + #custom message counter + self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero + + #steering_wheel_stalk last position, used by ACC and ALCA + self.steering_wheel_stalk = None + + #variables used by ACC + + # vEgo kalman filter dt = 0.01 # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) @@ -154,6 +218,14 @@ def __init__(self, CP): K=np.matrix([[0.12287673], [0.29666309]])) self.v_ego = 0.0 + self.imperial_speed_units = True + + # The current max allowed cruise speed. Actual cruise speed sent to the car + # may be lower, depending on traffic. + self.v_cruise_pcm = 0.0 + # Actual cruise speed currently active on the car. + self.v_cruise_actual = 0.0 + def update(self, cp, epas_cp): # copy can_valid @@ -171,6 +243,13 @@ def update(self, cp, epas_cp): self.prev_left_blinker_on = self.left_blinker_on self.prev_right_blinker_on = self.right_blinker_on + self.steering_wheel_stalk = cp.vl["STW_ACTN_RQ"] + self.cruise_setting = cp.vl["STW_ACTN_RQ"]['SpdCtrlLvr_Stat'] + self.cruise_buttons = cp.vl["STW_ACTN_RQ"]['SpdCtrlLvr_Stat'] + + + + # ******************* parse out can ******************* self.door_all_closed = not any([cp.vl["GTW_carState"]['DOOR_STATE_FL'], cp.vl["GTW_carState"]['DOOR_STATE_FR'], cp.vl["GTW_carState"]['DOOR_STATE_RL'], cp.vl["GTW_carState"]['DOOR_STATE_RR']]) #JCT @@ -180,6 +259,7 @@ def update(self, cp, epas_cp): # TODO: Use values from DBC to parse this field self.steer_error = epas_cp.vl["EPAS_sysStatus"]['EPAS_steeringFault'] == 1 self.steer_not_allowed = epas_cp.vl["EPAS_sysStatus"]['EPAS_eacStatus'] not in [2,1] # 2 "EAC_ACTIVE" 1 "EAC_AVAILABLE" 3 "EAC_FAULT" 0 "EAC_INHIBITED" + self.steer_warning = self.steer_not_allowed self.brake_error = 0 #NOT WORKINGcp.vl[309]['ESP_brakeLamp'] #JCT # JCT More ESP errors available, these needs to be added once car steers on its own to disable / alert driver self.esp_disabled = 0 #NEED TO CORRECT DBC cp.vl[309]['ESP_espOffLamp'] or cp.vl[309]['ESP_tcOffLamp'] or cp.vl[309]['ESP_tcLampFlash'] or cp.vl[309]['ESP_espFaultLamp'] #JCT @@ -198,7 +278,7 @@ def update(self, cp, epas_cp): v_ego_x = self.v_ego_kf.update(speed) self.v_ego = float(v_ego_x[0]) self.a_ego = float(v_ego_x[1]) - + # this is a hack for the interceptor. This is now only used in the simulation # TODO: Replace tests by toyota so this can go away self.user_gas = 0 #for now @@ -206,11 +286,9 @@ def update(self, cp, epas_cp): can_gear_shifter = cp.vl["DI_torque2"]['DI_gear'] self.gear = 0 # JCT - self.angle_steers = -(cp.vl["STW_ANGLHP_STAT"]['StW_AnglHP']) #JCT polarity reversed from Honda/Acura + self.angle_steers = -(cp.vl["STW_ANGLHP_STAT"]['StW_AnglHP']) #JCT polarity reversed from Honda/Acura self.angle_steers_rate = 0 #JCT - self.cruise_setting = cp.vl["STW_ACTN_RQ"]['SpdCtrlLvr_Stat'] - self.cruise_buttons = cp.vl["STW_ACTN_RQ"]['SpdCtrlLvr_Stat'] self.blinker_on = (cp.vl["STW_ACTN_RQ"]['TurnIndLvr_Stat'] == 1) or (cp.vl["STW_ACTN_RQ"]['TurnIndLvr_Stat'] == 2) self.left_blinker_on = cp.vl["STW_ACTN_RQ"]['TurnIndLvr_Stat'] == 1 self.right_blinker_on = cp.vl["STW_ACTN_RQ"]['TurnIndLvr_Stat'] == 2 @@ -235,17 +313,19 @@ def update(self, cp, epas_cp): # brake switch has shown some single time step noise, so only considered when # switch is on for at least 2 consecutive CAN samples - self.brake_switch = epas_cp.vl["EPAS_sysStatus"]['EPAS_eacErrorCode'] == 3 and epas_cp.vl["EPAS_sysStatus"]['EPAS_eacStatus'] == 0 #cp.vl["DI_torque2"]['DI_brakePedal'] - self.brake_pressed = epas_cp.vl["EPAS_sysStatus"]['EPAS_eacErrorCode'] == 3 and epas_cp.vl["EPAS_sysStatus"]['EPAS_eacStatus'] == 0 #cp.vl["DI_torque2"]['DI_brakePedal'] + # Todo / refactor: This shouldn't have to do with epas == 3.. + # was wrongly set to epas_cp.vl["EPAS_sysStatus"]['EPAS_eacErrorCode'] == 3 and epas_cp.vl["EPAS_sysStatus"]['EPAS_eacStatus'] == 0 + self.brake_switch = cp.vl["DI_torque2"]['DI_brakePedal'] + self.brake_pressed = cp.vl["DI_torque2"]['DI_brakePedal'] - self.user_brake = cp.vl["DI_torque2"]['DI_brakePedal'] self.standstill = cp.vl["DI_torque2"]['DI_vehicleSpeed'] == 0 - if cp.vl["DI_state"]['DI_speedUnits'] == 0: - self.v_cruise_pcm = (cp.vl["DI_state"]['DI_cruiseSet'])*CV.MPH_TO_KPH # Reported in MPH, expected in KPH?? - else: - self.v_cruise_pcm = cp.vl["DI_state"]['DI_cruiseSet'] - self.pcm_acc_status = cp.vl["DI_state"]['DI_cruiseState'] + self.imperial_speed_units = cp.vl["DI_state"]['DI_speedUnits'] == 0 + + if self.imperial_speed_units: + self.v_cruise_actual = (cp.vl["DI_state"]['DI_cruiseSet'])*CV.MPH_TO_KPH # Reported in MPH, expected in KPH?? + else: + self.v_cruise_actual = cp.vl["DI_state"]['DI_cruiseSet'] self.hud_lead = 0 #JCT self.cruise_speed_offset = calc_cruise_offset(self.v_cruise_pcm, self.v_ego) diff --git a/selfdrive/car/tesla/custom_alert.py b/selfdrive/car/tesla/custom_alert.py new file mode 100644 index 00000000000000..5f232c9dd59c78 --- /dev/null +++ b/selfdrive/car/tesla/custom_alert.py @@ -0,0 +1,17 @@ + +alert_file_path = "/data/openpilot/selfdrive/car/tesla/alert.msg" +alert_file_rw = "w" + +def custom_alert_message(message,CS,duration): + fo = open(alert_file_path, alert_file_rw) + fo.write(message) + fo.close() + CS.custom_alert_counter = duration + + +def update_custom_alert(CS): + if (CS.custom_alert_counter > 0): + CS.custom_alert_counter -= 1 + if (CS.custom_alert_counter ==0): + custom_alert_message("",CS,0) + CS.custom_alert_counter = -1 diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 7e2743e7dcfa4a..ac7f795ad230cf 100644 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -208,8 +208,7 @@ def update(self, c): ret.gasPressed = self.CS.user_gas_pressed # brake pedal - ret.brake = self.CS.user_brake - ret.brakePressed = self.CS.brake_pressed != 0 + ret.brakePressed = (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 @@ -277,13 +276,8 @@ def update(self, c): 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 @@ -298,9 +292,11 @@ def update(self, c): else: self.can_invalid_count = 0 if self.CS.steer_error: - events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.WARNING])) + if self.CS.cstm_btns.get_button_status("steer") == 0: + events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.WARNING])) elif self.CS.steer_warning: - events.append(create_event('steerTempUnavailableMute', [ET.NO_ENTRY, ET.WARNING])) + if self.CS.cstm_btns.get_button_status("steer") == 0: + events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CS.brake_error: events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if not ret.gearShifter == 'drive': @@ -324,11 +320,17 @@ def update(self, c): 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('steerTempUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - #Note: This event is thrown for steering override (needs more refactoring) +# 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: #break not canceling when pressed + self.CS.cstm_btns.set_button_status("brake", 2 if ret.brakePressed 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])) diff --git a/selfdrive/car/tesla/snd/attention.wav b/selfdrive/car/tesla/snd/attention.wav index 8b987c77c43c17..2e9981e4d03020 100644 Binary files a/selfdrive/car/tesla/snd/attention.wav and b/selfdrive/car/tesla/snd/attention.wav differ diff --git a/selfdrive/car/tesla/snd/disable.wav b/selfdrive/car/tesla/snd/disable.wav index 7d007a10a2bda6..ecc82aa8e2a841 100644 Binary files a/selfdrive/car/tesla/snd/disable.wav and b/selfdrive/car/tesla/snd/disable.wav differ diff --git a/selfdrive/car/tesla/snd/enable.wav b/selfdrive/car/tesla/snd/enable.wav index 2c9cf6c715b933..91ada8773fe38e 100644 Binary files a/selfdrive/car/tesla/snd/enable.wav and b/selfdrive/car/tesla/snd/enable.wav differ diff --git a/selfdrive/car/tesla/snd/error.wav b/selfdrive/car/tesla/snd/error.wav index b44c938aaa27c9..81154c225ddd36 100644 Binary files a/selfdrive/car/tesla/snd/error.wav and b/selfdrive/car/tesla/snd/error.wav differ diff --git a/selfdrive/car/tesla/snd/info.wav b/selfdrive/car/tesla/snd/info.wav index 8375aa413702b9..97f71bf8738c53 100644 Binary files a/selfdrive/car/tesla/snd/info.wav and b/selfdrive/car/tesla/snd/info.wav differ diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py index d8ff6db16fce63..8edb25bba3b322 100644 --- a/selfdrive/car/tesla/teslacan.py +++ b/selfdrive/car/tesla/teslacan.py @@ -69,39 +69,140 @@ def create_das_status_msg(autopilotState,idx): struct.pack_into('B', msg, msg_len-1, add_tesla_checksum(msg_id,msg)) return [msg_id, 0, msg.raw, 0] -def create_cruise_adjust_msg(spdCtrlLvr_stat, idx, lastStalkMsg): - """Creates a CAN message from the cruise control stalk. +def create_DAS_info_msg(mid): + msg_id = 0x539 + msg_len = 8 + msg = create_string_buffer(msg_len) + if (mid == 0): + struct.pack_into('BBBBBBBB', msg, 0, 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00) + elif (mid == 1): + struct.pack_into('BBBBBBBB', msg, 0, 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00) + elif (mid == 2): + struct.pack_into('BBBBBBBB', msg, 0, 0x0a,0x01,0x11,0x00,0x00,0x00,0x8b,0x00) + elif (mid == 3): + struct.pack_into('BBBBBBBB', msg, 0, 0x0b,0x00,0x07,0x01,0x01,0x00,0x00,0x00) + elif (mid == 4): + struct.pack_into('BBBBBBBB', msg, 0, 0x0d,0x00,0x00,0x00,0xa8,0x6a,0xbd,0xc9) + elif (mid == 5): + struct.pack_into('BBBBBBBB', msg, 0, 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00) + elif (mid == 6): + struct.pack_into('BBBBBBBB', msg, 0, 0x11,0x00,0x00,0x00,0x00,0x00,0x00,0x00) + elif (mid == 7): + struct.pack_into('BBBBBBBB', msg, 0, 0x12,0xca,0x0a,0x51,0x21,0xf4,0x38,0xd3) + elif (mid == 8): + struct.pack_into('BBBBBBBB', msg, 0, 0x13,0x01,0xff,0xff,0xff,0xfc,0x00,0x00) + else: + struct.pack_into('BBBBBBBB', msg, 0, 0x14,0x05,0x00,0x00,0x91,0x53,0x86,0x6a) + return [msg_id, 0, msg.raw, 0] + - Simluates pressing the cruise control stalk (STW_ACTN_RQ.SpdCtrlLvr_Stat +def create_DAS_status_msg(idx): + msg_id = 0x399 + msg_len = 8 + msg = create_string_buffer(msg_len) + struct.pack_into('BBBBBBB', msg, 0, 0x03,0x0b,0x20,0x00,0x08,0x08,(idx << 4) + 8) + struct.pack_into('B', msg, msg_len-1, add_tesla_checksum(msg_id,msg)) + return [msg_id, 0, msg.raw, 0] + +def create_DAS_status2_msg(idx): + msg_id = 0x389 + msg_len = 8 + msg = create_string_buffer(msg_len) + struct.pack_into('BBBBBBB', msg, 0, 0xff,0x03,0x44,0x04,0x00,0x80,(idx << 4) ) + struct.pack_into('B', msg, msg_len-1, add_tesla_checksum(msg_id,msg)) + return [msg_id, 0, msg.raw, 0] + +def create_DAS_bodyControls_msg(idx): + msg_id = 0x3E9 + msg_len = 8 + msg = create_string_buffer(msg_len) + struct.pack_into('BBBBBBB', msg, 0, 0xf1,0x0c,0x00,0x00,0x00,0x00,(idx << 4) ) + struct.pack_into('B', msg, msg_len-1, add_tesla_checksum(msg_id,msg)) + return [msg_id, 0, msg.raw, 0] +def create_DAS_pscControl_msg(idx): + msg_id = 0x219 + msg_len = 8 + msg = create_string_buffer(msg_len) + struct.pack_into('BBBBBBBB', msg, 0, 0x90 + idx,0x00,0x00,0x00,0x00,0x00,0x00,0x00 ) + struct.pack_into('B', msg, 2, add_tesla_checksum(msg_id,msg)) + return [msg_id, 0, msg.raw, 0] + +def create_DAS_lanes_msg(idx): + msg_id = 0x239 + msg_len = 8 + msg = create_string_buffer(msg_len) + struct.pack_into('BBBBBBBB', msg, 0, 0x62,0x28,0x62,0x7b,0x65,0x7d,0x30,0x2c) + return [msg_id, 0, msg.raw, 0] + +def create_DAS_objects_msg(idx): + msg_id = 0x309 + msg_len = 8 + msg = create_string_buffer(msg_len) + struct.pack_into('BBBBBBBB', msg, 0, 0x01,0xff,0xff,0xff,0x83,0xff,0xff,0x03) + return [msg_id, 0, msg.raw, 0] + + +def create_cruise_adjust_msg(spdCtrlLvr_stat, turnIndLvr_Stat, real_steering_wheel_stalk): + """Creates a CAN message from the cruise control stalk. + Simluates pressing the cruise control stalk (STW_ACTN_RQ.SpdCtrlLvr_Stat) + and turn signal stalk (STW_ACTN_RQ.TurnIndLvr_Stat) It is probably best not to flood these messages so that the real stalk works normally. - Args: spdCtrlLvr_stat: Int value of dbc entry STW_ACTN_RQ.SpdCtrlLvr_Stat + (allowing us to simulate pressing the cruise stalk up or down) + -1 means no change + TurnIndLvr_Stat: Int value of dbc entry STW_ACTN_RQ.TurnIndLvr_Stat + (allowing us to simulate pressing the turn signal up or down) + -1 means no change + real_steering_wheel_stalk: Previous STW_ACTN_RQ message sent by the real + stalk. When sending these artifical messages for cruise control, we want + to mimic whatever windshield wiper and highbeam settings the car is + currently sending. + """ msg_id = 0x045 # 69 in hex, STW_ACTN_RQ msg_len = 8 msg = create_string_buffer(msg_len) - if ( True ): #(len(lastStalkMsg) == 0): - b0 = (spdCtrlLvr_stat << 2) & 0xFF - b1 = 0xFF - b2 = 0x00 - b3 = 0x00 - b4 = 0x00 - b5 = 0x00 - b6 = 0x02 + (idx << 4) - else: - #b0 = ( ord(lastStalkMsg[0]) & 0xC0 ) + spdCtrlLvr_stat - b0 = ( ord(lastStalkMsg[0]) & 0x80 ) + ( 1 << 6 ) + spdCtrlLvr_stat - b1 = ord(lastStalkMsg[1]) - b2 = ord(lastStalkMsg[2]) - b3 = ord(lastStalkMsg[3]) - b4 = ord(lastStalkMsg[4]) - b5 = ord(lastStalkMsg[5]) - idx = ((((ord(lastStalkMsg[6]) & 0xF0) >> 4) + 1 ) & 0x0F) - b6 = ord(lastStalkMsg[6]) & 0x0F + (idx << 4) - struct.pack_into('BBBBBBB', msg, 0, b0, b1, b2, b3, b4, b5, b6) - struct.pack_into('B', msg, msg_len-1, add_tesla_crc(msg,7)) + # Do not send messages that conflict with the driver's actual actions on the + # steering wheel stalk. To ensure this, copy all the fields you can from the + # real cruise stalk message. + fake_stalk = real_steering_wheel_stalk.copy() + + if spdCtrlLvr_stat > -1: + # if accelerating, override VSL_Enbl_Rq to 1. + if spdCtrlLvr_stat in [4, 16]: + fake_stalk['VSL_Enbl_Rq'] = 1 + fake_stalk['SpdCtrlLvr_Stat'] = spdCtrlLvr_stat + # message count should be 1 more than the previous (and loop after 16) + if turnIndLvr_Stat > -1: + fake_stalk['TurnIndLvr_Stat'] = turnIndLvr_Stat + fake_stalk['MC_STW_ACTN_RQ'] = (int(round(fake_stalk['MC_STW_ACTN_RQ'])) + 1) % 16 + # CRC should initially be 0 before a new one is calculated. + fake_stalk['CRC_STW_ACTN_RQ'] = 0 + + # Set the first byte, containing cruise control + struct.pack_into('B', msg, 0, + (fake_stalk['SpdCtrlLvr_Stat']) + + (int(round(fake_stalk['VSL_Enbl_Rq'])) << 6)) + # Set the 2nd byte, containing DTR_Dist_Rq + struct.pack_into('B', msg, 1, fake_stalk['DTR_Dist_Rq']) + # Set the 3rd byte, containing turn indicator, highbeams, and wiper wash + struct.pack_into('B', msg, 2, + int(round(fake_stalk['TurnIndLvr_Stat'])) + + (int(round(fake_stalk['HiBmLvr_Stat'])) << 2) + + (int(round(fake_stalk['WprWashSw_Psd'])) << 4) + + (int(round(fake_stalk['WprWash_R_Sw_Posn_V2'])) << 6) + ) + # Set the 7th byte, containing the wipers and message counter. + struct.pack_into('B', msg, 6, + int(round(fake_stalk['WprSw6Posn'])) + + (fake_stalk['MC_STW_ACTN_RQ'] << 4)) + + # Finally, set the CRC for the message. Must be calculated last! + fake_stalk['CRC_STW_ACTN_RQ'] = add_tesla_crc(msg=msg, msg_len=7) + struct.pack_into('B', msg, msg_len-1, fake_stalk['CRC_STW_ACTN_RQ']) + return [msg_id, 0, msg.raw, 0] diff --git a/selfdrive/car/tesla/uibuttons.py b/selfdrive/car/tesla/uibuttons.py new file mode 100644 index 00000000000000..480ba25442c98e --- /dev/null +++ b/selfdrive/car/tesla/uibuttons.py @@ -0,0 +1,133 @@ +#library to work with buttons and ui.c via buttons.msg file +import struct +from ctypes import create_string_buffer +import os +from datetime import datetime + +buttons_labels_path = "/data/openpilot/selfdrive/car/tesla/buttons.msg" +buttons_status_in_path = "/data/openpilot/selfdrive/car/tesla/buttons.ui.msg" +buttons_status_out_path = "/data/openpilot/selfdrive/car/tesla/buttons.cc.msg" +buttons_file_rw = "wb" +buttons_file_r = "rb" +btn_msg_len = 23 +btn_msg_struct = "6s6s11s" #name=5 char string, label = 5 char string, satus = 1 char, label2 = 10 char string + +class UIButton: + def __init__(self,btn_name,btn_label,btn_status,btn_label2): + self.btn_name = btn_name + self.btn_label = btn_label + self.btn_label2 = btn_label2 + self.btn_status = btn_status + + +class UIButtons: + def write_buttons_labels_to_file(self): + fo = open(buttons_labels_path, buttons_file_rw) + for btn in self.btns: + fo.write(struct.pack(btn_msg_struct,btn.btn_name,btn.btn_label,btn.btn_label2)) + fo.close() + + def read_buttons_labels_from_file(self): + fi = open(buttons_labels_path, buttons_file_r) + indata = fi.read() + fi.close() + if len(indata) == btn_msg_len * 6 : + #we have all the data + self.btns = [] + for i in range(0, len(indata), btn_msg_len): + name,label,label2 = struct.unpack(btn_msg_struct, indata[i:i+btn_msg_len]) + self.btns.append(UIButton(name.rstrip("\0"),label.rstrip("\0"),0,label2.rstrip("\0"))) + #now read the last saved statuses + else: + #we don't have all the data, ignore + print "labels file is bad" + + def read_buttons_in_file(self): + modification_date = datetime.fromtimestamp(os.path.getmtime(buttons_status_in_path)) + if (modification_date > self.last_in_read_time): + fi = open(buttons_status_in_path, buttons_file_r) + indata = fi.read() + fi.close() + if len(indata) == 6: + for i in range(0,len(indata)): + if self.btns[i].btn_status > 0: + if (i == 1) and (ord(indata[1])==48): + #don't change status, just model + if (self.btns[i].btn_label2 == "Mod OP"): + self.btns[i].btn_label2 = "Mod JJ" + else: + self.btns[i].btn_label2 = "Mod OP" + self.write_buttons_labels_to_file() + else: + self.btns[i].btn_status = (ord(indata[i]) - 48) * self.btns[i].btn_status + else: + self.btns[i].btn_status = ord(indata[i]) - 48 + self.last_in_read_time = modification_date + self.hasChanges = True + else: + #something wrong with the file + print "status file is bad" + + def write_buttons_in_file(self): + fo = open(buttons_status_in_path, buttons_file_rw) + for btn in self.btns: + btn_val = 1 if btn.btn_status > 0 else 0 + fo.write(struct.pack("B",btn_val + 48)) + fo.close() + + def write_buttons_out_file(self): + if self.hasChanges: + fo = open(buttons_status_out_path, buttons_file_rw) + for btn in self.btns: + fo.write(struct.pack("B",btn.btn_status + 48)) + fo.close() + self.hasChanges = False + + def read_buttons_out_file(self): + fi = open(buttons_status_out_path, buttons_file_r) + indata = fi.read() + fi.close() + if len(indata) == 6: + for i in range(0,len(indata)): + self.btns[i].btn_status = ord(indata[i]) - 48 + else: + #something wrong with the file + print "status file is bad" + + + def __init__(self): + self.btns = [] + self.hasChanges = True + self.last_in_read_time = datetime.min + if os.path.exists(buttons_labels_path): + #there is a file, load it + self.read_buttons_labels_from_file() + self.read_buttons_out_file() + self.read_buttons_in_file() + else: + #there is no file, create it + self.btns.append(UIButton("alca","ALC",0,"")) + self.btns.append(UIButton("acc","ACC",0,"Mod OP")) + self.btns.append(UIButton("pedal","PDL",0,"")) + self.btns.append(UIButton("steer","STR",0,"")) + self.btns.append(UIButton("brake","BRK",1,"")) + self.btns.append(UIButton("sound","SND",1,"")) + self.write_buttons_labels_to_file() + self.write_buttons_in_file() + self.write_buttons_out_file() + + def get_button_status(self,btn_name): + ret_val =-1 + for i in range(0,6): + if self.btns[i].btn_name.strip() == btn_name: + ret_val = self.btns[i].btn_status + return ret_val + + def set_button_status(self,btn_name,btn_status): + for i in range(0,6): + if self.btns[i].btn_name.strip() == btn_name: + self.btns[i].btn_status = btn_status + self.hasChanges = True + + + diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py index f34f361bcf126f..870bcfb1657f0e 100644 --- a/selfdrive/car/tesla/values.py +++ b/selfdrive/car/tesla/values.py @@ -19,10 +19,13 @@ class CAR: # Car button codes class CruiseButtons: # VAL_ 69 SpdCtrlLvr_Stat 32 "DN_1ST" 16 "UP_1ST" 8 "DN_2ND" 4 "UP_2ND" 2 "RWD" 1 "FWD" 0 "IDLE" ; - RES_ACCEL = 16 - DECEL_SET = 32 - CANCEL = 1 - MAIN = 2 + RES_ACCEL = 16 + RES_ACCEL_2ND = 4 + DECEL_SET = 32 + DECEL_2ND = 8 + CANCEL = 1 + MAIN = 2 + IDLE = 0 #car chimes: enumeration from dbc file. Chimes are for alerts and warnings diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index c709fb2dedf796..53de99b7e95f6b 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -5,6 +5,8 @@ #include #include #include +#include +#include #include @@ -89,6 +91,13 @@ const int alert_sizes[] = { [ALERTSIZE_FULL] = vwp_h, }; +typedef struct UICstmButton { + char btn_name[6]; + char btn_label[6]; + char btn_label2[11]; +} UICstmButton; + + typedef struct UIScene { int frontview; @@ -143,7 +152,6 @@ typedef struct UIScene { // Used to display calibration progress int cal_status; int cal_perc; - // Used to show gps planner status bool gps_planner_active; @@ -159,7 +167,17 @@ typedef struct UIState { EGLSurface surface; NVGcontext *vg; - + //BB + int custom_alert_playsound; + UICstmButton btns[6]; + char btns_status[6]; + char *car_model; + int btns_x[6]; + int btns_y[6]; + int btns_r[6]; + time_t label_last_modified; + time_t status_last_modified; + //BB END int font_courbd; int font_sans_regular; int font_sans_semibold; @@ -328,7 +346,11 @@ static void ui_init(UIState *s) { pthread_mutex_init(&s->lock, NULL); pthread_cond_init(&s->bg_cond, NULL); - + // + s->status = STATUS_DISENGAGED; + s->car_model = "tesla"; + s->label_last_modified = 0; + s->status_last_modified = 0; // init connections s->thermal_sock = zsock_new_sub(">tcp://127.0.0.1:8005", ""); @@ -368,6 +390,7 @@ static void ui_init(UIState *s) { s->plus_sock_raw = zsock_resolve(s->plus_sock); s->ipc_fd = -1; + s->custom_alert_playsound=0; // init display s->fb = framebuffer_init("ui", 0x00010000, true, @@ -878,6 +901,7 @@ static void ui_draw_world(UIState *s) { } } + //BB START: functions added for the display of various items static int bb_ui_draw_measure(UIState *s, const char* bb_value, const char* bb_uom, const char* bb_label, int bb_x, int bb_y, int bb_uom_dx, @@ -915,6 +939,231 @@ static int bb_ui_draw_measure(UIState *s, const char* bb_value, const char* bb_ return (int)((bb_valueFontSize + bb_labelFontSize)*2.5) + 5; } +static bool bb_handle_ui_touch(UIState *s, int touch_x, int touch_y) { + char *out_status_file = malloc(90); + sprintf(out_status_file,"/data/openpilot/selfdrive/car/%s/buttons.ui.msg",s->car_model); + char temp_stats[6]; + int oFile; + for(int i=0; i<6; i++) { + if (s->btns_r[i] > 0) { + if ((abs(touch_x - s->btns_x[i]) < s->btns_r[i]) && (abs(touch_y - s->btns_y[i]) < s->btns_r[i])) { + //found it; change the status and write to file + if (s->btns_status[i] > 0) { + s->btns_status[i] = 0; + } else { + s->btns_status[i] = 1; + } + //now write to file + for (int j=0; j<6; j++) { + if (s->btns_status[j] ==0) { + temp_stats[j]='0'; + } else { + temp_stats[j]='1'; + } + } + oFile = open(out_status_file,O_WRONLY); + if (oFile != -1) { + write(oFile,&temp_stats,6); + } + close(oFile); + //done, return true + return true; + } + } + } + return false; +}; + +static int bb_get_button_status(UIState *s, char *btn_name) { + int ret_status = 0; + for (int i = 0; i< 6; i++) { + if (strcmp(s->btns[i].btn_name,btn_name)==0) { + ret_status = s->btns_status[i]; + } + } + return ret_status; +} + +static void bb_draw_button(UIState *s, int btn_id) { + const UIScene *scene = &s->scene; + + int viz_button_x = 0; + const int viz_button_y = (box_y + (bdr_s*1.5)) + 20; + const int viz_button_w = 140; + const int viz_button_h = 140; + + char *btn_text, *btn_text2; + + const int delta_x = viz_button_w * 1.1; + + if (btn_id >2) { + viz_button_x = scene->ui_viz_rx + scene->ui_viz_rw - (bdr_s*2) -190; + viz_button_x -= (6-btn_id) * delta_x ; + } else { + viz_button_x = scene->ui_viz_rx + (bdr_s*2) + 200; + viz_button_x += (btn_id) * delta_x; + } + + btn_text = s->btns[btn_id].btn_label; + btn_text2 = s->btns[btn_id].btn_label2; + + if (strcmp(btn_text,"")==0) { + s->btns_r[btn_id] = 0; + } else { + s->btns_r[btn_id]= (int)((viz_button_w + viz_button_h)/4); + } + s->btns_x[btn_id]=viz_button_x + s->btns_r[btn_id]; + s->btns_y[btn_id]=viz_button_y + s->btns_r[btn_id]; + if (s->btns_r[btn_id] == 0) { + return; + } + + nvgBeginPath(s->vg); + nvgRoundedRect(s->vg, viz_button_x, viz_button_y, viz_button_w, viz_button_h, 80); + nvgStrokeWidth(s->vg, 12); + + + if (s->btns_status[btn_id] ==0) { + //disabled - red + nvgStrokeColor(s->vg, nvgRGBA(255, 0, 0, 200)); + if (strcmp(btn_text2,"")==0) { + btn_text2 = "Off"; + } + } else + if (s->btns_status[btn_id] ==1) { + //enabled - white + nvgStrokeColor(s->vg, nvgRGBA(255,255,255,200)); + nvgStrokeWidth(s->vg, 4); + if (strcmp(btn_text2,"")==0) { + btn_text2 = "Ready"; + } + } else + if (s->btns_status[btn_id] ==2) { + //active - green + nvgStrokeColor(s->vg, nvgRGBA(28, 204,98,200)); + if (strcmp(btn_text2,"")==0) { + btn_text2 = "Active"; + } + } else + if (s->btns_status[btn_id] ==9) { + //available - thin white + nvgStrokeColor(s->vg, nvgRGBA(200,200,200,40)); + nvgStrokeWidth(s->vg, 4); + if (strcmp(btn_text2,"")==0) { + btn_text2 = ""; + } + } else { + //others - orange + nvgStrokeColor(s->vg, nvgRGBA(255, 188, 3, 200)); + if (strcmp(btn_text2,"")==0) { + btn_text2 = "Alert"; + } + } + + nvgStroke(s->vg); + + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + nvgFontFace(s->vg, "sans-regular"); + nvgFontSize(s->vg, 14*2.5); + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200)); + nvgText(s->vg, viz_button_x+viz_button_w/2, 210, btn_text2, NULL); + + nvgFontFace(s->vg, "sans-semibold"); + nvgFontSize(s->vg, 28*2.5); + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgText(s->vg, viz_button_x+viz_button_w/2, 183, btn_text, NULL); +} + +static void bb_draw_buttons(UIState *s) { + const UIScene *scene = &s->scene; + char *labels_file = malloc(90); + char *in_status_file = malloc(90); + + sprintf(labels_file,"/data/openpilot/selfdrive/car/%s/buttons.msg",s->car_model); + sprintf(in_status_file,"/data/openpilot/selfdrive/car/%s/buttons.cc.msg",s->car_model); + + int lFile; + int sFile; + char temp_stats[6]; + struct stat filestat; + int file_status; + bool changes_present; + + changes_present = false; + file_status = stat(labels_file, &filestat); + //read only if modified after last read + if ((filestat.st_mtime > s->label_last_modified) || (s->label_last_modified ==0)) { + lFile = open (labels_file, O_RDONLY); + if (lFile != -1) { + int rd = read(lFile, &(s->btns), 6*sizeof(struct UICstmButton)); + close(lFile); + s->label_last_modified = filestat.st_mtime; + changes_present = true; + } + } + file_status = stat(in_status_file, &filestat); + //read only if modified after last read + if ((filestat.st_mtime > s->status_last_modified) || (s->status_last_modified ==0)) { + sFile = open(in_status_file, O_RDONLY); + if (sFile != -1) { + int rd = read(sFile, &(temp_stats),6*sizeof(char)); + if (rd == 6) { + for (int i = 0; i < 6; i++) { + s->btns_status[i] = temp_stats[i]-'0'; + } + } + close(sFile); + s->status_last_modified = filestat.st_mtime; + changes_present = true; + } + } + for (int i = 0; i < 6; i++) { + bb_draw_button(s,i); + } +} + +static void bb_ui_draw_custom_alert(UIState *s) { + const UIScene *scene = &s->scene; + char *filepath = malloc(90); + sprintf(filepath,"/data/openpilot/selfdrive/car/%s/alert.msg",s->car_model); + //get 3-state switch position + int alert_msg_fd; + char alert_msg[1000]; + if (strlen(s->scene.alert_text1) > 0) { + //already a system alert, ignore ours + return; + } + alert_msg_fd = open (filepath, O_RDONLY); + //if we can't open then done + if (alert_msg_fd == -1) { + s->custom_alert_playsound = 0; + return; + } else { + int rd = read(alert_msg_fd, &(s->scene.alert_text1), 1000); + if ((rd > 1) && (s->scene.alert_text1[rd-1] == '^')) { + //^ as last character means warning + if (s->custom_alert_playsound==0) { + //sound never played, request sound + s->custom_alert_playsound=1; + } + //update_status(s,3); //ALERT_WARNINGa + s->status = 3; //ALERT_WARNING + rd --; + } + s->scene.alert_text1[rd] = '\0'; + close(alert_msg_fd); + if (strlen(s->scene.alert_text1) > 0) { + s->scene.alert_size = ALERTSIZE_SMALL; + } else { + s->scene.alert_size = ALERTSIZE_NONE; + s->scene.alert_text1[0]=0; + s->custom_alert_playsound = 0; + } + } +} + + + static void bb_ui_draw_measures_left(UIState *s, int bb_x, int bb_y, int bb_w ) { const UIScene *scene = &s->scene; int bb_rx = bb_x + (int)(bb_w/2); @@ -1236,6 +1485,7 @@ static void bb_ui_draw_UI(UIState *s) { tri_state_switch = buffer[0] -48; close(tri_state_fd); } + if (tri_state_switch == 1) { const UIScene *scene = &s->scene; const int bb_dml_w = 180; @@ -1245,9 +1495,13 @@ static void bb_ui_draw_UI(UIState *s) { const int bb_dmr_w = 180; const int bb_dmr_x = scene->ui_viz_rx + scene->ui_viz_rw - bb_dmr_w - (bdr_s*2) ; const int bb_dmr_y = (box_y + (bdr_s*1.5))+220; - bb_ui_draw_measures_left(s,bb_dml_x, bb_dml_y, bb_dml_w ); bb_ui_draw_measures_right(s,bb_dmr_x, bb_dmr_y, bb_dmr_w ); + bb_draw_buttons(s); + bb_ui_draw_custom_alert(s); + } + if (tri_state_switch ==2) { + bb_ui_draw_custom_alert(s); } if (tri_state_switch ==3) { ui_draw_vision_grid(s); @@ -1257,6 +1511,26 @@ static void bb_ui_draw_UI(UIState *s) { //BB END: functions added for the display of various items +static void update_status(UIState *s, int status) { + int old_status = s->status; + if (s->status != status) { + s->status = status; + // wake up bg thread to change + pthread_cond_signal(&s->bg_cond); + //if tesla call the sound command + if ((status ==3 ) && (s->custom_alert_playsound > 0)) { + return; + } + char* snd_command; + if ((status ==3 ) && (s->custom_alert_playsound == 1)) { + s->custom_alert_playsound = 2; + } + if ((bb_get_button_status(s,"sound") > 0) && ((old_status != STATUS_STOPPED) || (s->status != STATUS_DISENGAGED))) { + asprintf(&snd_command, "python /data/openpilot/selfdrive/car/%s/snd/playsound.py %d &",s->car_model, status); + system(snd_command); + } + } +} static void ui_draw_vision_maxspeed(UIState *s) { const UIScene *scene = &s->scene; @@ -1576,18 +1850,6 @@ static ModelData read_model(cereal_ModelData_ptr modelp) { return d; } -static void update_status(UIState *s, int status) { - if (s->status != status) { - s->status = status; - // wake up bg thread to change - pthread_cond_signal(&s->bg_cond); - //if tesla call the sound command - char* snd_command; - asprintf(&snd_command, "python /data/openpilot/selfdrive/car/tesla/snd/playsound.py %d &", status); - system(snd_command); - } -} - static void ui_update(UIState *s) { int err; @@ -2216,6 +2478,8 @@ int main() { if (touched == 1) { // touch event will still happen :( set_awake(s, true); + // BB check touch area + bb_handle_ui_touch(s,touch_x,touch_y); } // manage wakefulness