diff --git a/.gitignore b/.gitignore index 70ad45c3202958..ab2ae04b12bc95 100644 --- a/.gitignore +++ b/.gitignore @@ -32,3 +32,7 @@ selfdrive/test/tests/plant/out /src/ one + +.vscode/*.json +launch_chffrplus.sh + diff --git a/.travis.yml b/.travis.yml index 74411b5de82281..8a5e83bbdf73c4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -10,3 +10,7 @@ script: - docker run --rm -v "$(pwd)"/selfdrive/test/tests/plant/out:/tmp/openpilot/selfdrive/test/tests/plant/out tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/tests/plant && OPTEST=1 ./test_longitudinal.py' + +branches: + only: + - /.*/ diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000000000..d873a158a895b7 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,7 @@ +{ + "files.associations": { + "algorithm": "cpp", + "chrono": "cpp", + "limits": "cpp" + } +} \ No newline at end of file diff --git a/cereal/Makefile b/cereal/Makefile index dc6b7f9d51fa03..318d99ad063ec9 100644 --- a/cereal/Makefile +++ b/cereal/Makefile @@ -1,14 +1,14 @@ PWD := $(shell pwd) -SRCS := log.capnp car.capnp +SRCS := log.capnp car.capnp ui.capnp -GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++ -JS := gen/js/car.capnp.js gen/js/log.capnp.js +GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++ gen/cpp/ui.capnp.c++ +JS := gen/js/car.capnp.js gen/js/log.capnp.js gen/js/ui.capnp.js UNAME_M ?= $(shell uname -m) # only generate C++ for docker tests ifneq ($(OPTEST),1) - GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/include/c++.capnp.h gen/c/include/java.capnp.h + GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/ui.capnp.c gen/c/include/c++.capnp.h gen/c/include/java.capnp.h ifeq ($(UNAME_M),x86_64) ifneq (, $(shell which capnpc-java)) diff --git a/cereal/__init__.py b/cereal/__init__.py index 2d3b48526b2c34..2638b0c07520fd 100644 --- a/cereal/__init__.py +++ b/cereal/__init__.py @@ -6,3 +6,4 @@ log = capnp.load(os.path.join(CEREAL_PATH, "log.capnp")) car = capnp.load(os.path.join(CEREAL_PATH, "car.capnp")) +ui = capnp.load(os.path.join(CEREAL_PATH, "ui.capnp")) diff --git a/cereal/ui.capnp b/cereal/ui.capnp new file mode 100644 index 00000000000000..db5a42f22b26d3 --- /dev/null +++ b/cereal/ui.capnp @@ -0,0 +1,64 @@ +using Cxx = import "./include/c++.capnp"; +$Cxx.namespace("cereal"); + +using Java = import "./include/java.capnp"; +$Java.package("ai.comma.openpilot.cereal"); +$Java.outerClassname("Ui"); + +using Car = import "car.capnp"; + +@0xce6ca45dddcd5317; + +struct UIButtonInfo { + # button ID 0..5 + btnId @0 :Int8; + # internal button name + btnName @1 :Text; + # display label for button (3 chars) + btnLabel @2 :Text; + # buttons status: 0 = DISABLED, 1 = AVAILABLE, 2 = ENABLED, 3 = WARNING, 9 = NOT AVAILABLE + btnStatus @3 :Int16; + # small font label shows below the main label, max 7 chars + btnLabel2 @4 :Text; +} + +struct UIButtonStatus { + # button ID 0..5 + btnId @0 :Int8; + # buttons status: 0 = DISABLED, 1 = AVAILABLE, 2 = ENABLED, 3 = WARNING, 9 = NOT AVAILABLE + btnStatus @1 :Int16; +} + +struct UICustomAlert { + caStatus @0 :Int8; + caText @1 :Text; +} + +struct UISetCar { + icCarFolder @0 :Text; + icCarName @1 :Text; +} + +struct UIPlaySound { + sndSound @0 :Int8; +} + +struct UIUpdate { + uiDoUpdate @0 :Int8; + uiStatus @1 :Int8; + uiCanDisplayMessage @2 :Int8; +} + +#struct UIEvent { +# # in nanoseconds? +# logMonoTime @0 :UInt64; +# +# union { +# uiButtonInfo @1 :UIButtonInfo; +# uiCustomAlert @2 :UICustomAlert; +# uiSetCar @3 :UISetCar; +# uiButtonStatus @4 :UIButtonStatus; +# uiUpdate @5 :UIUpdate; +# uiPlaySound @6 :UIPlaySound; +# } +#} diff --git a/common/dbc.py b/common/dbc.py index 6accad43f82bec..6e64afad2d49b8 100755 --- a/common/dbc.py +++ b/common/dbc.py @@ -212,6 +212,7 @@ def decode(self, x, arr=None, debug=False): st = x[2].ljust(8, '\x00') le, be = None, None + size = msg[0][1] for s in msg[1]: if arr is not None and s[0] not in arr: diff --git a/common/fingerprints.py b/common/fingerprints.py index d4845f8d416a0a..8be6bfad465f5b 100644 --- a/common/fingerprints.py +++ b/common/fingerprints.py @@ -61,4 +61,4 @@ def eliminate_incompatible_cars(msg, candidate_cars): def all_known_cars(): """Returns a list of all known car strings.""" - return _FINGERPRINTS.keys() + return _FINGERPRINTS.keys() \ No newline at end of file diff --git a/common/params.py b/common/params.py index cc137d5a7b15c4..ed37312c8238ce 100755 --- a/common/params.py +++ b/common/params.py @@ -358,4 +358,4 @@ def put(self, key, dat): # Test multiprocess: # seq 0 100000 | xargs -P20 -I{} python common/params.py DongleId {} && sleep 0.05 - # while python common/params.py DongleId; do sleep 0.05; done + # while python common/params.py DongleId; do sleep 0.05; done \ No newline at end of file diff --git a/opendbc/honda_accord_s2t_2018_can_generated.dbc b/opendbc/honda_accord_s2t_2018_can_generated.dbc index 43a46aff831d0c..006c7608e1da4d 100644 --- a/opendbc/honda_accord_s2t_2018_can_generated.dbc +++ b/opendbc/honda_accord_s2t_2018_can_generated.dbc @@ -39,8 +39,8 @@ BU_: EBCM EON CAM RADAR PCM EPS VSA SCM BDY XXX EPB BO_ 228 STEERING_CONTROL: 5 EON SG_ STEER_TORQUE_REQUEST : 23|1@0+ (1,0) [0|1] "" EPS - SG_ SET_ME_X00 : 22|7@0+ (1,0) [0|127] "" EPS - SG_ SET_ME_X00_2 : 31|8@0+ (1,0) [0|0] "" EPS + SG_ GERNBY1 : 22|7@0+ (1,0) [0|127] "" EPS + SG_ GERNBY_2 : 31|8@0+ (1,0) [0|0] "" EPS SG_ STEER_TORQUE : 7|16@0- (1,0) [-4096|4096] "" EPS SG_ COUNTER : 37|2@0+ (1,0) [0|3] "" EPS SG_ CHECKSUM : 35|4@0+ (1,0) [0|15] "" EPS diff --git a/opendbc/tesla_can.dbc b/opendbc/tesla_can.dbc index 37d839e44671c3..0c482cb1de32b2 100644 --- a/opendbc/tesla_can.dbc +++ b/opendbc/tesla_can.dbc @@ -98,7 +98,8 @@ BO_ 3 STW_ANGL_STAT: 8 STW SG_ MC_STW_ANGL_STAT : 55|4@0+ (1,0) [0|15] "" NEO SG_ CRC_STW_ANGL_STAT : 63|8@0+ (1,0) [0|255] "" NEO -BO_ 14 STW_ANGLHP_STAT: 8 STW +BO_ 14 STW_ANGLHP_STAT: 8 STW + SG_ StW_AnglHP : 5|14@0+ (0.1,-819.2) [-819.2|819] "deg" NEO SG_ StW_AnglHP_Spd : 21|14@0+ (0.5,-4096) [-4096|4095.5] "deg/s" NEO SG_ StW_AnglHP_Sens_Stat : 33|2@0+ (1,0) [0|0] "" NEO @@ -147,19 +148,25 @@ BO_ 309 ESP_135h: 5 ESP SG_ ESP_espOffLamp : 31|1@0+ (1,0) [-1|2] "" NEO SG_ ESP_stabilityControlSts : 14|3@0+ (1,0) [6|7] "" NEO SG_ ESP_tcLampFlash : 5|1@0+ (1,0) [-1|2] "" NEO - SG_ ESP_tcOffLamp : 15|1@0+ (1,0) [0|1] "" NEO BO_ 341 ESP_B: 8 ESP - SG_ ESP_BChecksum : 39|8@0+ (1,0) [0|255] "" NEO,EPAS - SG_ ESP_BCounter : 62|4@0+ (1,0) [1|15] "" NEO,EPAS - SG_ ESP_vehicleSpeed : 47|16@0+ (0.00999999978,0) [0|0] "kph" NEO,EPAS - SG_ ESP_vehicleSpeedQF : 57|2@0+ (1,0) [1|2] "" NEO,EPAS - SG_ ESP_wheelPulseCountFrL : 7|8@0+ (1,0) [0|254] "" NEO,EPAS - SG_ ESP_wheelPulseCountFrR : 15|8@0+ (1,0) [0|254] "" NEO,EPAS - SG_ ESP_wheelPulseCountReL : 23|8@0+ (1,0) [0|254] "" NEO,EPAS + SG_ ESP_tcOffLamp : 15|1@0+ (1,0) [0|1] "" NEO + +BO_ 341 ESP_B: 8 ESP + SG_ ESP_BChecksum : 39|8@0+ (1,0) [0|255] "" NEO,EPAS + SG_ ESP_BCounter : 62|4@0+ (1,0) [1|15] "" NEO,EPAS + SG_ ESP_vehicleSpeed : 47|16@0+ (0.00999999978,0) [0|0] "kph" NEO,EPAS + SG_ ESP_vehicleSpeedQF : 57|2@0+ (1,0) [1|2] "" NEO,EPAS + SG_ ESP_wheelPulseCountFrL : 7|8@0+ (1,0) [0|254] "" NEO,EPAS + SG_ ESP_wheelPulseCountFrR : 15|8@0+ (1,0) [0|254] "" NEO,EPAS + SG_ ESP_wheelPulseCountReL : 23|8@0+ (1,0) [0|254] "" NEO,EPAS SG_ ESP_wheelPulseCountReR : 31|8@0+ (1,0) [0|254] "" NEO,EPAS -BO_ 532 EPB_epasControl: 3 EPB - SG_ EPB_epasControlChecksum : 23|8@0+ (1,0) [0|255] "" NEO,EPAS - SG_ EPB_epasControlCounter : 11|4@0+ (1,0) [0|15] "" NEO,EPAS +BO_ 513 SDM1: 5 GTW + SG_ SDM_bcklPassStatus : 3|2@0+ (1,0) [0|3] "" NEO + SG_ SDM_bcklDrivStatus : 5|2@0+ (1,0) [0|3] "" NEO + +BO_ 532 EPB_epasControl: 3 EPB + SG_ EPB_epasControlChecksum : 23|8@0+ (1,0) [0|255] "" NEO,EPAS + SG_ EPB_epasControlCounter : 11|4@0+ (1,0) [0|15] "" NEO,EPAS SG_ EPB_epasEACAllow : 2|3@0+ (1,0) [4|7] "" NEO,EPAS BO_ 792 GTW_carState: 8 GTW @@ -307,6 +314,50 @@ BO_ 840 GTW_status: 8 GTW SG_ GTW_statusChecksum : 63|8@0+ (1,0) [0|255] "" NEO SG_ GTW_statusCounter : 51|4@0+ (1,0) [0|15] "" NEO +BO_ 1361 GAS_COMMAND: 6 EON + SG_ GAS_COMMAND : 7|16@0+ (0.0507968128,-22.85856576) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND2 : 23|16@0+ (0.1015936256,-22.85856576) [0|1] "" INTERCEPTOR + SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR + SG_ IDX : 35|4@0+ (1,0) [0|15] "" INTERCEPTOR + SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" INTERCEPTOR + +BO_ 1362 GAS_SENSOR: 6 INTERCEPTOR + SG_ INTERCEPTOR_GAS : 7|16@0+ (0.0507968128,-22.85856576) [0|1] "" EON + SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.1015936256,-22.85856576) [0|1] "" EON + SG_ STATE : 35|4@0+ (1,0) [0|15] "" EON + SG_ IDX : 39|4@0+ (1,0) [0|15] "" EON + SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" EON + +BO_ 920 GTW_carConfig: 8 GTW + SG_ GTW_performanceConfig : 2|3@0+ (1,0) [0|0] "" NEO + SG_ GTW_fourWheelDrive : 4|2@0+ (1,0) [0|0] "" NEO + SG_ GTW_unknown1 : 5|1@0+ (1,0) [0|0] "" NEO + SG_ GTW_dasHw : 7|2@0+ (1,0) [0|0] "" NEO + SG_ GTW_parkAssistInstalled : 9|2@0+ (1,0) [0|0] "" NEO + SG_ GTW_forwardRadarHw : 11|2@0+ (1,0) [0|0] "" NEO + SG_ GTW_airSuspensionInstalled : 14|3@0+ (1,0) [0|0] "" NEO + SG_ GTW_unknown2 : 15|1@0+ (1,0) [0|0] "" NEO + SG_ GTW_country : 23|16@0+ (1,0) [0|0] "" NEO + SG_ GTW_parkSensorGeometryType : 33|2@0+ (1,0) [0|0] "" NEO + SG_ GTW_rhd : 34|1@0+ (1,0) [0|0] "" NEO + SG_ GTW_bodyControlsType : 35|1@0+ (1,0) [0|0] "" NEO + SG_ GTW_radarPosition : 39|4@0+ (1,0) [0|0] "" NEO + SG_ GTW_rearCornerRadarHw : 41|2@0+ (1,0) [0|0] "" NEO + SG_ GTW_frontCornerRadarHw : 43|2@0+ (1,0) [0|0] "" NEO + SG_ GTW_epasType : 45|2@0+ (1,0) [0|0] "" NEO + SG_ GTW_chassisType : 47|2@0+ (1,0) [0|2] "" NEO + SG_ GTW_wheelType : 52|5@0+ (1,0) [0|0] "" NEO + SG_ GTW_rearSeatControllerMask : 55|3@0+ (1,0) [0|7] "" NEO + SG_ GTW_euVehicle : 56|1@0+ (1,0) [0|0] "" NEO + SG_ GTW_foldingMirrorsInstalled : 57|1@0+ (1,0) [0|0] "" NEO + SG_ GTW_brakeHwType : 59|2@0+ (1,0) [0|2] "" NEO + SG_ GTW_autopilot : 61|2@0+ (1,0) [0|0] "" NEO + SG_ GTW_unknown3 : 63|2@0+ (1,0) [0|0] "" NEO + + + + + VAL_ 3 StW_Angl 16383 "SNA" ; VAL_ 3 StW_AnglSens_Id 2 "MUST" 0 "PSBL" 1 "SELF" ; VAL_ 3 StW_AnglSens_Stat 2 "ERR" 3 "ERR_INI" 1 "INI" 0 "OK" ; @@ -417,5 +468,6 @@ VAL_ 904 MCU_clusterReadyForDrive 0 "NO_SNA" 1 "YES" ; VAL_ 1160 DAS_steeringAngleRequest 16384 "ZERO_ANGLE" ; VAL_ 1160 DAS_steeringControlType 1 "ANGLE_CONTROL" 3 "DISABLED" 0 "NONE" 2 "RESERVED" ; VAL_ 1160 DAS_steeringHapticRequest 1 "ACTIVE" 0 "IDLE" ; +VAL_ 1362 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; CM_ "CHFFR_METRIC 1160 DAS_steeringAngleRequest STEER_ANGLE 0.1098666 180; CHFFR_METRIC 264 DI_motorRPM ENGINE_RPM 1 0"; diff --git a/panda/board/drivers/can.h b/panda/board/drivers/can.h index b837094c9a0d38..8ba8eff717575c 100644 --- a/panda/board/drivers/can.h +++ b/panda/board/drivers/can.h @@ -53,7 +53,7 @@ int can_push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) { ret = 1; } exit_critical_section(); - if (ret == 0) puts("can_push failed!\n"); + //if (ret == 0) puts("can_push failed!\n"); return ret; } diff --git a/panda/board/drivers/gmlan_alt.h b/panda/board/drivers/gmlan_alt.h index 8521100a88083a..6c1dec4223daec 100644 --- a/panda/board/drivers/gmlan_alt.h +++ b/panda/board/drivers/gmlan_alt.h @@ -161,6 +161,7 @@ void reset_gmlan_switch_timeout(void) { can_timeout_counter = GMLAN_TICKS_PER_SECOND; gmlan_switch_below_timeout = 1; gmlan_alt_mode = GPIO_SWITCH; + TIM4->CR1 = TIM_CR1_CEN; // Enable timer again in case we were timed out } void set_bitbanged_gmlan(int val) { @@ -253,6 +254,10 @@ void TIM4_IRQHandler(void) { } TIM4->SR = 0; } //gmlan switch mode + else { + //disable timer; not a valid mode + TIM4->CR1 &= ~TIM_CR1_CEN; + } } void bitbang_gmlan(CAN_FIFOMailBox_TypeDef *to_bang) { diff --git a/panda/board/drivers/lin.h b/panda/board/drivers/lin.h new file mode 100644 index 00000000000000..75ca4414594d2c --- /dev/null +++ b/panda/board/drivers/lin.h @@ -0,0 +1,175 @@ +//-------------------------------------------------------------- +// LIN Defines +//-------------------------------------------------------------- +#define LIN_SYNC_DATA 0x55 // SyncField (do not change) +#define LIN_MAX_DATA 8 // max 8 databytes + + +//-------------------------------------------------------------- +// LIN error messages +//-------------------------------------------------------------- +typedef enum { + LIN_OK = 0, // no error + LIN_WRONG_LEN, // wrong number of data + LIN_RX_EMPTY, // no frame received + LIN_WRONG_CRC // Checksum wrong +}LIN_ERR_t; + +//-------------------------------------------------------------- +// LIN frame Struct +//-------------------------------------------------------------- +typedef struct { + uint8_t has_response; // set to 1 if message expects a response; else set to 0 + uint8_t frame_id; // ID number of the frame + uint8_t data_len; // number of data bytes + uint8_t data[LIN_MAX_DATA]; // data +}LIN_FRAME_t; + +uint8_t p_LIN_makeChecksum(LIN_FRAME_t *frame); +void USART_SendBreak(uart_ring *u); + + +// -------------------------------------------------------------- +// sends data via LIN interface +// frame: +// frame_id = Unique ID [0x00 to 0xFF] +// data_len = length of data to be sent +// data [] = data to be sent +// +// return_value: +// LIN_OK = Frame has been sent +// LIN_WRONG_LEN = invalid data length +// -------------------------------------------------------------- +LIN_ERR_t LIN_SendData(uart_ring *LIN_UART, LIN_FRAME_t *frame) +{ + uint8_t checksum,n; + + // check the length + if((frame -> data_len < 1) || (frame -> data_len > LIN_MAX_DATA)) { + return(LIN_WRONG_LEN); + } + + // calculate checksum + checksum=p_LIN_makeChecksum(frame); + + //------------------------ + // Break-Field + //------------------------ + USART_SendBreak(LIN_UART); + + //------------------------ + // Sync-Field + //------------------------ + putc(LIN_UART, LIN_SYNC_DATA); + + //------------------------ + // ID-Field + //------------------------ + putc(LIN_UART, frame->frame_id); + + //------------------------ + // Data-Field [1...n] + //------------------------ + for(n=0; n < frame -> data_len; n++) { + putc(LIN_UART, frame -> data[n]); + } + + //------------------------ + // CRC-Field + //------------------------ + putc(LIN_UART, checksum); + + return(LIN_OK); +} + + +// -------------------------------------------------------------- +// receives data via LIN interface +// frame: +// frame_id = Unique ID [0x00 to 0xFF] +// data_len = number of data to be received +// return: +// data [] = data received (if LIN_OK) +// +// return_value: +// LIN_OK = Frame was received +// LIN_WRONG_LEN = wrong number of data +// LIN_RX_EMPTY = no frame received +// LIN_WRONG_CRC = Checksum wrong +// -------------------------------------------------------------- +LIN_ERR_t LIN_SendReceiveFrame(uart_ring *LIN_UART, LIN_FRAME_t *frame) +{ + // check the length + if((frame -> data_len < 1) || (frame -> data_len > LIN_MAX_DATA)) { + return(LIN_WRONG_LEN); + } + + //------------------------------- + // Break-Field + //------------------------------- + USART_SendBreak(LIN_UART); + + //------------------------------- + // Sync-Field + //------------------------------- + putc(LIN_UART, LIN_SYNC_DATA); + + //------------------------------- + // ID-Field + //------------------------------- + putc(LIN_UART, frame->frame_id); + + //now wait for the slave device to respond with the data + + return(LIN_OK); +} + +LIN_ERR_t LIN_ReceiveData(uart_ring *LIN_UART, LIN_FRAME_t *frame) +{ + //------------------------------- + // copy received data + //------------------------------- + + //TODO: Get receive working. Do I need to add a small delay here? + + uint8_t *resp = 0; + int resp_len = 0; + + while ((resp_len < frame -> data_len) && getc(LIN_UART, (char*)&resp[resp_len])) { + ++resp_len; + frame->data[resp_len]=resp; + } + + return(LIN_OK); +} + +void USART_SendBreak(uart_ring *u) +{ + SET_BIT(u -> uart -> CR1, USART_CR1_SBK); +} + +// -------------------------------------------------------------- +// internal function +// Calculate checksum over all data +// (classic-mode = inverted modulo256 sum) +// +// ret_value = checksum +//-------------------------------------------------------------- +uint8_t p_LIN_makeChecksum(LIN_FRAME_t *frame) +{ + uint8_t ret_value=0, n; + uint16_t dummy; + + // calculate checksum + dummy = 0; + for(n = 0; n < frame -> data_len; n++) { + dummy += frame -> data[n]; + if (dummy > 0xFF) { + dummy -= 0xFF; + } + } + ret_value = (uint8_t)(dummy); + ret_value ^= 0xFF; + + return(ret_value); +} diff --git a/panda/board/drivers/uja1023.h b/panda/board/drivers/uja1023.h new file mode 100644 index 00000000000000..4b566195515880 --- /dev/null +++ b/panda/board/drivers/uja1023.h @@ -0,0 +1,224 @@ +/* +This driver configures the NXP UJA1023 chip on revB tesla giraffe +for output on startup and then allows the user to set the output +pins via set_uja1023_output_bits() and set_uja1023_output_buffer() + +Set UJA1023 output pins. Bits = pins +P0 = bit 0 +P1 = bit 1 +... +P7 = bit 7 +0b10101010 = P0 off, P1 on, P2 off, P3 on, P4 off, P5 on, P6 off, P7 on + +Examples: +set bit 5: +set_uja1023_output_bits(1 << 5); //turn on any pins that = 1, leave all other pins alone + +clear bit 5: +clear_uja1023_output_bits(1 << 5); //turn off any pins that = 1, leave all other pins alone + +Or set the whole buffer at once: +set_uja1023_output_buffer(0xFF); //turn all output pins on +set_uja1023_output_buffer(0x00); //turn all output pins off +*/ + +#define TICKS_PER_FRAME 2000 //1s = 33000 + +#ifdef PANDA +int lin_send_timer = TICKS_PER_FRAME; + +LIN_FRAME_t assign_id_frame, io_cfg_1_frame, io_cfg_2_frame, io_cfg_3_frame, io_cfg_4_frame, px_req_frame, frame_to_send, frame_to_receive; + + +int initial_loop_num = 0; + + +void TIM5_IRQHandler(void) { + if (TIM5->SR & TIM_SR_UIF) { + if (lin_send_timer == 0) { + //send every 1s + lin_send_timer = TICKS_PER_FRAME; + if (initial_loop_num <= 3) { + switch(initial_loop_num) { + case 0 : + frame_to_send = assign_id_frame; + puts("UJA1023 init stage 0\n"); + break; + case 1 : + frame_to_send = io_cfg_1_frame; + puts("UJA1023 init stage 1\n"); + break; + case 2 : + frame_to_send = io_cfg_2_frame; + puts("UJA1023 init stage 2\n"); + break; + case 3 : + frame_to_send = io_cfg_3_frame; + puts("UJA1023 init stage 3\n"); + break; + } + initial_loop_num++; + } + else { + + /* + if (px_req_frame.data[0] == 0x02) { + px_req_frame.data[0] = 0x00; + } + else { + px_req_frame.data[0] = 0x02; + } + */ + frame_to_send = px_req_frame; + } + LIN_SendData(&lin2_ring, &frame_to_send); + /* + puts("Set UJA to output: "); + puth(px_req_frame.data[0]); + puts("\n"); + */ + + + } //if counter = 0 + + if (lin_send_timer == (TICKS_PER_FRAME / 10) && frame_to_send.has_response) { + //send empty header to slave + LIN_SendReceiveFrame(&lin2_ring, &frame_to_receive); + } + + if (lin_send_timer == ((TICKS_PER_FRAME / 10) * 2) && frame_to_send.has_response) { + LIN_ReceiveData(&lin2_ring, &frame_to_receive); + puts("Received Lin frame: "); + + /* + for(int n=0; n < frame_to_receive.data_len; n++) { + puth(frame_to_receive.data[n]); + } + */ + puts("\n"); + } + + lin_send_timer--; + } //interrupt + TIM5->SR = 0; +} + +void uja1023_init(void) { + //make receive frame + //LIN_FRAME_t frame_to_receive; + frame_to_receive.data_len = 8; + frame_to_receive.frame_id = 0x7D; //Response PID, 0x7D = 2 bit parity + 0x3C raw ID + + //make frame for Assign frame ID + //LIN_FRAME_t assign_id_frame; + assign_id_frame.has_response = 1; + assign_id_frame.data_len = 8; + assign_id_frame.frame_id = 0x3C; //0x3C is for diagnostic frames + assign_id_frame.data[0] = 0x60; //D0, iniital node address, set to 0x60 default + assign_id_frame.data[1] = 0x06; //D1, protocol control info (PCI); should be 0x06 + assign_id_frame.data[2] = 0xB1; //D2, service id (SID); should be 0xB4 + assign_id_frame.data[3] = 0x11; //D3, supplier ID low byte; should be 0x11 + assign_id_frame.data[4] = 0x00; //D4, supplier ID high byte; should be 0x00 + assign_id_frame.data[5] = 0x00; //D5, function id low byte; should be 0x00 + assign_id_frame.data[6] = 0x00; //D6, function id high byte; should be 0x00 + assign_id_frame.data[7] = 0x04; //D7, slave node address (NAD). 0x04 means ID(PxReq) = 04, ID(PxResp) = 05 + + //make frame for io_cfg_1; configure Px pins for push pull level output + //LIN_FRAME_t io_cfg_1_frame; + io_cfg_1_frame.has_response = 1; + io_cfg_1_frame.data_len = 8; + io_cfg_1_frame.frame_id = 0x3C; //0x3C is for diagnostic frames + io_cfg_1_frame.data[0] = 0x60; //D0, slave node address (NAD) (default 0x60) + io_cfg_1_frame.data[1] = 0x06; //D1, protocol control info (PCI); should be 0x06 + io_cfg_1_frame.data[2] = 0xB4; //D2, service id (SID); should be 0xB4 + io_cfg_1_frame.data[3] = 0x00; //D3, bits 7-6 are 00 for first cfg block. Bits 5-0 are for IM/INH, RxDL, ADCIN cfg. D3 default is 0x00 + io_cfg_1_frame.data[4] = 0xFF; //D4, High side enable (HSE). Set to 0xFF for High side driver or push pull + io_cfg_1_frame.data[5] = 0x00; //D4, Low side enable (LSE). Set to 0xFF for Low side driver or push pull + io_cfg_1_frame.data[6] = 0x00; //D6, Output mode (low byte) (OM0). Set to 0x00 for level output + io_cfg_1_frame.data[7] = 0x00; //D7, Output mode (high byte) (OM1). Set to 0x00 for level output + + //make frame for io_cfg_2; defaults + //LIN_FRAME_t io_cfg_2_frame; + io_cfg_2_frame.has_response = 1; + io_cfg_2_frame.data_len = 8; + io_cfg_2_frame.frame_id = 0x3C; //0x3C is for diagnostic frames + io_cfg_2_frame.data[0] = 0x60; //D0, slave node address (NAD) (default 0x60) + io_cfg_2_frame.data[1] = 0x06; //D1, protocol control info (PCI); should be 0x06 + io_cfg_2_frame.data[2] = 0xB4; //D2, service id (SID); should be 0xB4 + io_cfg_2_frame.data[3] = 0x40; //D3, bits 7-6 are 01 for second cfg block. Bits 5-0 are for LSLP, TxDL, SMC, SMW, SM. D3 default is 0x40 (every bit 0 except for 7-6) + io_cfg_2_frame.data[4] = 0x00; //D4, Capture Mode (low byte) CM0. Set to 0x00 for no capture + io_cfg_2_frame.data[5] = 0x00; //D5, Capture Mode (high byte) CM1. Set to 0x00 for no capture + io_cfg_2_frame.data[6] = 0x00; //D6, Threshold select TH2 TH1. Default 0x00 + io_cfg_2_frame.data[7] = 0x00; //D7, Local Wake up pin mask LWM. Default 0x00 + + //make frame for io_cfg_3; set LH value, classic checksum, and LIN speeds up to 20kbps + //LIN_FRAME_t io_cfg_3_frame; + io_cfg_3_frame.has_response = 1; + io_cfg_3_frame.data_len = 8; + io_cfg_3_frame.frame_id = 0x3C; //0x3C is for diagnostic frames + io_cfg_3_frame.data[0] = 0x60; //D0, slave node address (NAD) (default 0x60) + io_cfg_3_frame.data[1] = 0x04; //D1, protocol control info (PCI); should be 0x04 + io_cfg_3_frame.data[2] = 0xB4; //D2, service id (SID); should be 0xB4 + io_cfg_3_frame.data[3] = 0x80; //D3, bits 7-6 are 10 for third cfg block. Bits 5-0 are for LSC and ECC (classic vs enhanced checksum). D3 default is 0x80 (every bit 0 except for 7-6) + io_cfg_3_frame.data[4] = 0x00; //D4, Limp home mode output value. This is the value the output pins will go to if the bus times out or another error occurs + io_cfg_3_frame.data[5] = 0x10; //D5, PWM initial value; shouldn't matter but is 0x10 per datasheet example + io_cfg_3_frame.data[6] = 0xFF; //D6, Not used, set to 0xFF + io_cfg_3_frame.data[7] = 0xFF; //D7, Not used, set to 0xFF + + + //make frame for io_cfg_4; diagnostic data frame + //LIN_FRAME_t io_cfg_4_frame; + io_cfg_4_frame.has_response = 1; + io_cfg_4_frame.data_len = 8; + io_cfg_4_frame.frame_id = 0x3C; //0x3C is for diagnostic frames + io_cfg_4_frame.data[0] = 0x60; //D0, slave node address (NAD) (default 0x60) + io_cfg_4_frame.data[1] = 0x02; //D1, protocol control info (PCI); should be 0x02 + io_cfg_4_frame.data[2] = 0xB4; //D2, service id (SID); should be 0xB4 + io_cfg_4_frame.data[3] = 0xC0; //D3, bits 7-6 are 11 for fourth cfg block. Bits 5-0 are unused and should be set to 0; should be 0xC0 + io_cfg_4_frame.data[4] = 0xFF; //D4, Not used, set to 0xFF + io_cfg_4_frame.data[5] = 0xFF; //D5, Not used, set to 0xFF + io_cfg_4_frame.data[6] = 0xFF; //D6, Not used, set to 0xFF + io_cfg_4_frame.data[7] = 0xFF; //D7, Not used, set to 0xFF + + + //make frame for PxReq frame (this is what sets the UJA1023's output pins) + //LIN_FRAME_t px_req_frame; + px_req_frame.has_response = 0; + px_req_frame.data_len = 2; + px_req_frame.frame_id = 0xC4; //PID, 0xC4 = 2 bit parity + 0x04 raw ID + px_req_frame.data[0] = 0x00; //D0, bit 7 = P7, bit 6 = P6 ... bit 0 = P0. 0x02 would set P1 to on + px_req_frame.data[1] = 0x80; //D1, PWM Value; shouldn't matter but is 0x80 per datasheet example + + // setup + TIM5->PSC = 48-1; // tick on 1 us + TIM5->CR1 = TIM_CR1_CEN; // enable + TIM5->ARR = 30-1; // 33.3 kbps + + // in case it's disabled + NVIC_EnableIRQ(TIM5_IRQn); + + // run the interrupt + TIM5->DIER = TIM_DIER_UIE; // update interrupt + TIM5->SR = 0; + + initial_loop_num = 0; + +} + +//turn on any pins that = 1, leave all other pins alone +void set_uja1023_output_bits(uint8_t to_set) { + px_req_frame.data[0] |= to_set; +} + +//turn off any pins that = 1, leave all other pins alone +void clear_uja1023_output_bits(uint8_t to_clear) { + px_req_frame.data[0] &= ~to_clear; +} + +//Set the whole Px output buffer at once: +void set_uja1023_output_buffer(uint8_t to_set) { + px_req_frame.data[0] = to_set; +} + +#endif + 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/gpio.h b/panda/board/gpio.h index 6112f6f887c703..c9d8aab97c0b46 100644 --- a/panda/board/gpio.h +++ b/panda/board/gpio.h @@ -120,6 +120,7 @@ void periph_init() { RCC->APB1ENR |= RCC_APB1ENR_TIM2EN; RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; RCC->APB1ENR |= RCC_APB1ENR_TIM4EN; + RCC->APB1ENR |= RCC_APB1ENR_TIM5EN; RCC->APB2ENR |= RCC_APB2ENR_USART1EN; RCC->AHB2ENR |= RCC_AHB2ENR_OTGFSEN; RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; diff --git a/panda/board/main.c b/panda/board/main.c index 8bb6a0cee09681..15b689d60ff9e5 100644 --- a/panda/board/main.c +++ b/panda/board/main.c @@ -4,7 +4,6 @@ // ********************* includes ********************* #include "libc.h" -#include "safety.h" #include "provision.h" #include "drivers/drivers.h" @@ -13,12 +12,14 @@ #include "gpio.h" #include "drivers/uart.h" +#include "drivers/lin.h" #include "drivers/adc.h" #include "drivers/usb.h" #include "drivers/gmlan_alt.h" #include "drivers/can.h" #include "drivers/spi.h" #include "drivers/timer.h" +#include "safety.h" // ***************************** fan ***************************** @@ -291,6 +292,15 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) { can_silent = ALL_CAN_BUT_MAIN_SILENT; can_autobaud_enabled[0] = false; break; + case SAFETY_TESLA: + can_silent = ALL_CAN_LIVE; + can_autobaud_enabled[0] = false; + can_autobaud_enabled[1] = false; + #ifdef PANDA + can_autobaud_enabled[2] = false; + #endif + // MISSING: setup GMLAN pin as output and high level to switch EPAS CAN on Tesla Giraffe + break; default: can_silent = ALL_CAN_LIVE; can_autobaud_enabled[0] = false; diff --git a/panda/board/pedal.honda/.gitignore b/panda/board/pedal.honda/.gitignore new file mode 100644 index 00000000000000..94053f2925089b --- /dev/null +++ b/panda/board/pedal.honda/.gitignore @@ -0,0 +1 @@ +obj/* diff --git a/panda/board/pedal.honda/Makefile b/panda/board/pedal.honda/Makefile new file mode 100644 index 00000000000000..9917e381503b03 --- /dev/null +++ b/panda/board/pedal.honda/Makefile @@ -0,0 +1,59 @@ +# :set noet +PROJ_NAME = comma + +CFLAGS = -O2 -Wall -std=gnu11 -DPEDAL +CFLAGS += -mlittle-endian -mthumb -mcpu=cortex-m3 +CFLAGS += -msoft-float -DSTM32F2 -DSTM32F205xx +CFLAGS += -I ../inc -I ../ -I ../../ -nostdlib +CFLAGS += -T../stm32_flash.ld + +STARTUP_FILE = startup_stm32f205xx + +CC = arm-none-eabi-gcc +OBJCOPY = arm-none-eabi-objcopy +OBJDUMP = arm-none-eabi-objdump +DFU_UTIL = "dfu-util" + +# pedal only uses the debug cert +CERT = ../../certs/debug +CFLAGS += "-DALLOW_DEBUG" + +canflash: obj/$(PROJ_NAME).bin + ../../tests/pedal/enter_canloader.py $< + +usbflash: obj/$(PROJ_NAME).bin + ../../tests/pedal/enter_canloader.py; sleep 0.5 + PYTHONPATH=../../ python -c "from python import Panda; p = [x for x in [Panda(x) for x in Panda.list()] if x.bootstub]; assert(len(p)==1); p[0].flash('obj/$(PROJ_NAME).bin', reconnect=False)" + +recover: obj/bootstub.bin obj/$(PROJ_NAME).bin + ../../tests/pedal/enter_canloader.py --recover; sleep 0.5 + $(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08004000 -D obj/$(PROJ_NAME).bin + $(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08000000:leave -D obj/bootstub.bin + +obj/main.o: main.c ../*.h + mkdir -p obj + $(CC) $(CFLAGS) -o $@ -c $< + +obj/bootstub.o: ../bootstub.c ../*.h + mkdir -p obj + $(CC) $(CFLAGS) -o $@ -c $< + +obj/$(STARTUP_FILE).o: ../$(STARTUP_FILE).s + $(CC) $(CFLAGS) -o $@ -c $< + +obj/%.o: ../../crypto/%.c + $(CC) $(CFLAGS) -o $@ -c $< + +obj/$(PROJ_NAME).bin: obj/$(STARTUP_FILE).o obj/main.o + # hack + $(CC) -Wl,--section-start,.isr_vector=0x8004000 $(CFLAGS) -o obj/$(PROJ_NAME).elf $^ + $(OBJCOPY) -v -O binary obj/$(PROJ_NAME).elf obj/code.bin + SETLEN=1 ../../crypto/sign.py obj/code.bin $@ $(CERT) + +obj/bootstub.bin: obj/$(STARTUP_FILE).o obj/bootstub.o obj/sha.o obj/rsa.o + $(CC) $(CFLAGS) -o obj/bootstub.$(PROJ_NAME).elf $^ + $(OBJCOPY) -v -O binary obj/bootstub.$(PROJ_NAME).elf $@ + +clean: + rm -f obj/* + diff --git a/panda/board/pedal.honda/README b/panda/board/pedal.honda/README new file mode 100644 index 00000000000000..cf779db258fe14 --- /dev/null +++ b/panda/board/pedal.honda/README @@ -0,0 +1,28 @@ +This is the firmware for the comma pedal. It borrows a lot from panda. + +The comma pedal is a gas pedal interceptor for Honda/Acura. It allows you to "virtually" press the pedal. + +This is the open source software. Note that it is not ready to use yet. + +== Test Plan == + +* Startup +** Confirm STATE_FAULT_STARTUP +* Timeout +** Send value +** Confirm value is output +** Stop sending messages +** Confirm value is passthru after 100ms +** Confirm STATE_FAULT_TIMEOUT +* Random values +** Send random 6 byte messages +** Confirm random values cause passthru +** Confirm STATE_FAULT_BAD_CHECKSUM +* Same message lockout +** Send same message repeated +** Confirm timeout behavior +* Don't set enable +** Confirm no output +* Set enable and values +** Confirm output + diff --git a/panda/board/pedal.honda/main.c b/panda/board/pedal.honda/main.c new file mode 100644 index 00000000000000..5d5264791ccf54 --- /dev/null +++ b/panda/board/pedal.honda/main.c @@ -0,0 +1,295 @@ +//#define DEBUG +//#define CAN_LOOPBACK_MODE +//#define USE_INTERNAL_OSC + +#include "../config.h" + +#include "drivers/drivers.h" +#include "drivers/llgpio.h" +#include "gpio.h" + +#define CUSTOM_CAN_INTERRUPTS + +#include "libc.h" +#include "safety.h" +#include "drivers/adc.h" +#include "drivers/uart.h" +#include "drivers/dac.h" +#include "drivers/can.h" +#include "drivers/timer.h" + +#define CAN CAN1 + +//#define PEDAL_USB + +#ifdef PEDAL_USB + #include "drivers/usb.h" +#endif + +#define ENTER_BOOTLOADER_MAGIC 0xdeadbeef +uint32_t enter_bootloader_mode; + +void __initialize_hardware_early() { + early(); +} + +// ********************* serial debugging ********************* + +void debug_ring_callback(uart_ring *ring) { + char rcv; + while (getc(ring, &rcv)) { + putc(ring, rcv); + } +} + +#ifdef PEDAL_USB + +int usb_cb_ep1_in(uint8_t *usbdata, int len, int hardwired) { return 0; } +void usb_cb_ep2_out(uint8_t *usbdata, int len, int hardwired) {} +void usb_cb_ep3_out(uint8_t *usbdata, int len, int hardwired) {} +void usb_cb_enumeration_complete() {} + +int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) { + int resp_len = 0; + uart_ring *ur = NULL; + switch (setup->b.bRequest) { + // **** 0xe0: uart read + case 0xe0: + ur = get_ring_by_number(setup->b.wValue.w); + if (!ur) break; + if (ur == &esp_ring) uart_dma_drain(); + // read + while ((resp_len < min(setup->b.wLength.w, MAX_RESP_LEN)) && + getc(ur, (char*)&resp[resp_len])) { + ++resp_len; + } + break; + } + return resp_len; +} + +#endif + +// ***************************** honda can checksum ***************************** + +int can_cksum(uint8_t *dat, int len, int addr, int idx) { + int i; + int s = 0; + for (i = 0; i < len; i++) { + s += (dat[i] >> 4); + s += dat[i] & 0xF; + } + s += (addr>>0)&0xF; + s += (addr>>4)&0xF; + s += (addr>>8)&0xF; + s += idx; + s = 8-s; + return s&0xF; +} + +// ***************************** can port ***************************** + +// addresses to be used on CAN +#define CAN_GAS_INPUT 0x200 +#define CAN_GAS_OUTPUT 0x201 + +void CAN1_TX_IRQHandler() { + // clear interrupt + CAN->TSR |= CAN_TSR_RQCP0; +} + +// two independent values +uint16_t gas_set_0 = 0; +uint16_t gas_set_1 = 0; + +#define MAX_TIMEOUT 10 +uint32_t timeout = 0; +uint32_t current_index = 0; + +#define NO_FAULT 0 +#define FAULT_BAD_CHECKSUM 1 +#define FAULT_SEND 2 +#define FAULT_SCE 3 +#define FAULT_STARTUP 4 +#define FAULT_TIMEOUT 5 +#define FAULT_INVALID 6 +uint8_t state = FAULT_STARTUP; + +void CAN1_RX0_IRQHandler() { + while (CAN->RF0R & CAN_RF0R_FMP0) { + #ifdef DEBUG + puts("CAN RX\n"); + #endif + uint32_t address = CAN->sFIFOMailBox[0].RIR>>21; + if (address == CAN_GAS_INPUT) { + // softloader entry + if (CAN->sFIFOMailBox[0].RDLR == 0xdeadface) { + if (CAN->sFIFOMailBox[0].RDHR == 0x0ab00b1e) { + enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; + NVIC_SystemReset(); + } else if (CAN->sFIFOMailBox[0].RDHR == 0x02b00b1e) { + enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC; + NVIC_SystemReset(); + } + } + + // normal packet + uint8_t *dat = (uint8_t *)&CAN->sFIFOMailBox[0].RDLR; + uint8_t *dat2 = (uint8_t *)&CAN->sFIFOMailBox[0].RDHR; + uint16_t value_0 = (dat[0] << 8) | dat[1]; + uint16_t value_1 = (dat[2] << 8) | dat[3]; + uint8_t enable = (dat2[0] >> 7) & 1; + uint8_t index = (dat2[1] >> 4) & 3; + if (can_cksum(dat, 5, CAN_GAS_INPUT, index) == (dat2[1] & 0xF)) { + if (((current_index+1)&3) == index) { + #ifdef DEBUG + puts("setting gas "); + puth(value); + puts("\n"); + #endif + if (enable) { + gas_set_0 = value_0; + gas_set_1 = value_1; + } else { + // clear the fault state if values are 0 + if (value_0 == 0 && value_1 == 0) { + state = NO_FAULT; + } else { + state = FAULT_INVALID; + } + gas_set_0 = gas_set_1 = 0; + } + // clear the timeout + timeout = 0; + } + current_index = index; + } else { + // wrong checksum = fault + state = FAULT_BAD_CHECKSUM; + } + } + // next + CAN->RF0R |= CAN_RF0R_RFOM0; + } +} + +void CAN1_SCE_IRQHandler() { + state = FAULT_SCE; + can_sce(CAN); +} + +int pdl0 = 0, pdl1 = 0; +int pkt_idx = 0; + +int led_value = 0; + +void TIM3_IRQHandler() { + #ifdef DEBUG + puth(TIM3->CNT); + puts(" "); + puth(pdl0); + puts(" "); + puth(pdl1); + puts("\n"); + #endif + + // check timer for sending the user pedal and clearing the CAN + if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) { + uint8_t dat[8]; + dat[0] = (pdl0>>8)&0xFF; + dat[1] = (pdl0>>0)&0xFF; + dat[2] = (pdl1>>8)&0xFF; + dat[3] = (pdl1>>0)&0xFF; + dat[4] = state; + dat[5] = can_cksum(dat, 5, CAN_GAS_OUTPUT, pkt_idx) | (pkt_idx<<4); + CAN->sTxMailBox[0].TDLR = dat[0] | (dat[1]<<8) | (dat[2]<<16) | (dat[3]<<24); + CAN->sTxMailBox[0].TDHR = dat[4] | (dat[5]<<8); + CAN->sTxMailBox[0].TDTR = 6; // len of packet is 5 + CAN->sTxMailBox[0].TIR = (CAN_GAS_OUTPUT << 21) | 1; + ++pkt_idx; + pkt_idx &= 3; + } else { + // old can packet hasn't sent! + state = FAULT_SEND; + #ifdef DEBUG + puts("CAN MISS\n"); + #endif + } + + // blink the LED + set_led(LED_GREEN, led_value); + led_value = !led_value; + + TIM3->SR = 0; + + // up timeout for gas set + if (timeout == MAX_TIMEOUT) { + state = FAULT_TIMEOUT; + } else { + timeout += 1; + } +} + +// ***************************** main code ***************************** + +void pedal() { + // read/write + pdl0 = adc_get(ADCCHAN_ACCEL0); + pdl1 = adc_get(ADCCHAN_ACCEL1); + + // write the pedal to the DAC + if (state == NO_FAULT) { + dac_set(0, max(gas_set_0, pdl0)); + dac_set(1, max(gas_set_1, pdl1)); + } else { + dac_set(0, pdl0); + dac_set(1, pdl1); + } + + // feed the watchdog + IWDG->KR = 0xAAAA; +} + +int main() { + __disable_irq(); + + // init devices + clock_init(); + periph_init(); + gpio_init(); + +#ifdef PEDAL_USB + // enable USB + usb_init(); +#endif + + // pedal stuff + dac_init(); + adc_init(); + + // init can + can_silent = ALL_CAN_LIVE; + can_init(0); + + // 48mhz / 65536 ~= 732 + timer_init(TIM3, 15); + NVIC_EnableIRQ(TIM3_IRQn); + + // setup watchdog + IWDG->KR = 0x5555; + IWDG->PR = 0; // divider /4 + // 0 = 0.125 ms, let's have a 50ms watchdog + IWDG->RLR = 400 - 1; + IWDG->KR = 0xCCCC; + + puts("**** INTERRUPTS ON ****\n"); + __enable_irq(); + + // main pedal loop + while (1) { + pedal(); + } + + return 0; +} + diff --git a/panda/board/pedal.honda/obj/bootstub.bin b/panda/board/pedal.honda/obj/bootstub.bin new file mode 100755 index 00000000000000..30c7de8797a40c Binary files /dev/null and b/panda/board/pedal.honda/obj/bootstub.bin differ diff --git a/panda/board/pedal.honda/obj/bootstub.comma.elf b/panda/board/pedal.honda/obj/bootstub.comma.elf new file mode 100755 index 00000000000000..0eaa67e1c48053 Binary files /dev/null and b/panda/board/pedal.honda/obj/bootstub.comma.elf differ diff --git a/panda/board/pedal.honda/obj/code.bin b/panda/board/pedal.honda/obj/code.bin new file mode 100755 index 00000000000000..ec5f42bc0bafa7 Binary files /dev/null and b/panda/board/pedal.honda/obj/code.bin differ diff --git a/panda/board/pedal.honda/obj/comma.bin b/panda/board/pedal.honda/obj/comma.bin new file mode 100644 index 00000000000000..df9132cad895e6 Binary files /dev/null and b/panda/board/pedal.honda/obj/comma.bin differ diff --git a/panda/board/pedal.honda/obj/comma.elf b/panda/board/pedal.honda/obj/comma.elf new file mode 100755 index 00000000000000..cb4920bd8f718e Binary files /dev/null and b/panda/board/pedal.honda/obj/comma.elf differ diff --git a/panda/board/pedal/main.c b/panda/board/pedal/main.c index 5d5264791ccf54..9f189120c89a7e 100644 --- a/panda/board/pedal/main.c +++ b/panda/board/pedal/main.c @@ -20,13 +20,14 @@ #define CAN CAN1 +//#define PEDAL + //#define PEDAL_USB #ifdef PEDAL_USB #include "drivers/usb.h" #endif -#define ENTER_BOOTLOADER_MAGIC 0xdeadbeef uint32_t enter_bootloader_mode; void __initialize_hardware_early() { @@ -70,28 +71,22 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) { #endif -// ***************************** honda can checksum ***************************** - -int can_cksum(uint8_t *dat, int len, int addr, int idx) { +// ***************************** tesla pedal checksum ***************************** +int can_cksum(uint8_t *dat, int len, int addr) { int i; int s = 0; + s += ((addr)&0xFF) + ((addr>>8)&0xFF); for (i = 0; i < len; i++) { - s += (dat[i] >> 4); - s += dat[i] & 0xF; - } - s += (addr>>0)&0xF; - s += (addr>>4)&0xF; - s += (addr>>8)&0xF; - s += idx; - s = 8-s; - return s&0xF; + s = (s + dat[i]) & 0xFF; + } + return s; } // ***************************** can port ***************************** // addresses to be used on CAN -#define CAN_GAS_INPUT 0x200 -#define CAN_GAS_OUTPUT 0x201 +#define CAN_GAS_INPUT 0x551 +#define CAN_GAS_OUTPUT 0x552 void CAN1_TX_IRQHandler() { // clear interrupt @@ -139,9 +134,9 @@ void CAN1_RX0_IRQHandler() { uint16_t value_0 = (dat[0] << 8) | dat[1]; uint16_t value_1 = (dat[2] << 8) | dat[3]; uint8_t enable = (dat2[0] >> 7) & 1; - uint8_t index = (dat2[1] >> 4) & 3; - if (can_cksum(dat, 5, CAN_GAS_INPUT, index) == (dat2[1] & 0xF)) { - if (((current_index+1)&3) == index) { + uint8_t index = (dat2[0] ) & 0xF; + if (can_cksum(dat, 5, CAN_GAS_INPUT) == (dat2[1] & 0xFF)) { + if (((current_index+1)&0xF) == index) { #ifdef DEBUG puts("setting gas "); puth(value); @@ -200,14 +195,14 @@ void TIM3_IRQHandler() { dat[1] = (pdl0>>0)&0xFF; dat[2] = (pdl1>>8)&0xFF; dat[3] = (pdl1>>0)&0xFF; - dat[4] = state; - dat[5] = can_cksum(dat, 5, CAN_GAS_OUTPUT, pkt_idx) | (pkt_idx<<4); + dat[4] = ((state<<0) | (pkt_idx<<4))&0xFF; + dat[5] = can_cksum(dat, 5, CAN_GAS_OUTPUT); CAN->sTxMailBox[0].TDLR = dat[0] | (dat[1]<<8) | (dat[2]<<16) | (dat[3]<<24); CAN->sTxMailBox[0].TDHR = dat[4] | (dat[5]<<8); CAN->sTxMailBox[0].TDTR = 6; // len of packet is 5 CAN->sTxMailBox[0].TIR = (CAN_GAS_OUTPUT << 21) | 1; ++pkt_idx; - pkt_idx &= 3; + pkt_idx &= 0xF; } else { // old can packet hasn't sent! state = FAULT_SEND; @@ -239,8 +234,16 @@ void pedal() { // write the pedal to the DAC if (state == NO_FAULT) { - dac_set(0, max(gas_set_0, pdl0)); - dac_set(1, max(gas_set_1, pdl1)); + if (pdl0 > 500) { + dac_set(0, max(gas_set_0, pdl0)); + dac_set(1, max(gas_set_1, pdl1)); + } else if (gas_set_0 > 0) { + dac_set(0, gas_set_0); + dac_set(1, gas_set_1); + } else { + dac_set(0, pdl0); + dac_set(1, pdl1); + } } else { dac_set(0, pdl0); dac_set(1, pdl1); diff --git a/panda/board/safety.h b/panda/board/safety.h index 4d2b46899f7969..89a81042c6bf49 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -24,7 +24,7 @@ int max_limit_check(int val, const int MAX, const int MIN); int dist_to_meas_check(int val, int val_last, struct sample_t *val_meas, const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR); int driver_limit_check(int val, int val_last, struct sample_t *val_driver, - const int MAX, const int MAX_RATE_UP, const int MAX_RATE_DOWN, + const int MAX, const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ALLOWANCE, const int DRIVER_FACTOR); int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA); #ifdef PANDA @@ -117,6 +117,10 @@ const safety_hook_config safety_hook_registry[] = { {SAFETY_GM, &gm_hooks}, {SAFETY_FORD, &ford_hooks}, {SAFETY_CADILLAC, &cadillac_hooks}, +#ifdef PANDA + {SAFETY_TESLA, &tesla_hooks}, +#endif + {SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks}, {SAFETY_HYUNDAI, &hyundai_hooks}, {SAFETY_CHRYSLER, &chrysler_hooks}, {SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks}, diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h index a13e78f5a561c2..fceae1b8584eeb 100644 --- a/panda/board/safety/safety_tesla.h +++ b/panda/board/safety/safety_tesla.h @@ -7,53 +7,705 @@ // accel rising edge // brake rising edge // brake > 0mph -// -int fmax_limit_check(float val, const float MAX, const float MIN) { - return (val > MAX) || (val < MIN); -} // 2m/s are added to be less restrictive const struct lookup_t TESLA_LOOKUP_ANGLE_RATE_UP = { {2., 7., 17.}, - {5., .8, .25}}; + {25., 25., 25.}}; const struct lookup_t TESLA_LOOKUP_ANGLE_RATE_DOWN = { {2., 7., 17.}, - {5., 3.5, .8}}; + {25., 26., 25.}}; const struct lookup_t TESLA_LOOKUP_MAX_ANGLE = { {2., 29., 38.}, - {410., 92., 36.}}; + {500., 500., 500.}}; const int TESLA_RT_INTERVAL = 250000; // 250ms between real time checks +struct sample_t tesla_angle_meas; // last 3 steer angles + // state of angle limits -float tesla_desired_angle_last = 0; // last desired steer angle -float tesla_rt_angle_last = 0.; // last real time angle -float tesla_ts_angle_last = 0; +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; int tesla_speed = 0; +int current_car_time = -1; +int time_at_last_stalk_pull = -1; int eac_status = 0; int tesla_ignition_started = 0; +/* <-- revB giraffe GPIO */ +#include "../drivers/uja1023.h" + +uint32_t tesla_ts_brakelight_on_last = 0; +const int32_t BRAKELIGHT_CLEAR_INTERVAL = 250000; //25ms; needs to be slower than the framerate difference between the DI_torque2 (~100Hz) and DI_state messages (~10hz). +const int32_t STW_MENU_BTN_HOLD_INTERVAL = 750000; //75ms, how long before we recognize the user is holding this steering wheel button down + +uint32_t stw_menu_btn_pressed_ts = 0; +int stw_menu_current_output_state = 0; +int stw_menu_btn_state_last = 0; +int stw_menu_output_flag = 0; +int high_beam_lever_state = 0; +/* revB giraffe GPIO --> */ + +int tesla_radar_status = 0; //0-not present, 1-initializing, 2-active +uint32_t tesla_last_radar_signal = 0; +const int TESLA_RADAR_TIMEOUT = 1000000; // 1 second between real time checks +char radar_VIN[] = "5YJSA1H27FF087536"; //leave empty if your radar VIN matches the car VIN + +//EPB enable counter +int EPB_epasControl_idx = 0; + +//settings from bb_openpilot.cfg + +int enable_das_emulation = 0; +int enable_radar_emulation = 0; + +//fake DAS counters +int DAS_bootID_sent = 0; +int fake_DAS_counter = 0; +int DAS_object_idx = 0; +int DAS_control_idx = 0; +int DAS_pscControl_idx = 0; +int DAS_telemetryPeriodic_idx1 = 0; +int DAS_telemetryPeriodic_idx2 = 0; +int DAS_lanes_idx = 0; +int DAS_status_idx = 0; +int DAS_status2_idx = 0; +int DAS_bodyControls_idx = 0; +int DAS_info_idx = 0; +int DAS_warningMatrix0_idx = 0; +int DAS_warningMatrix1_idx = 0; +int DAS_warningMatrix3_idx = 0; +int DAS_steeringControl_idx = 0; +//fake DAS variables +int DAS_longC_enabled = 0; +int DAS_speed_limit_kph = 0; +int DAS_accel_min = 0; +int DAS_accel_max = 0; +int DAS_aeb_event = 0x00; +int DAS_acc_state = 0x00; +int DAS_jerk_min = 0x000; +int DAS_jerk_max = 0x0F; +int DAS_gas_to_resume = 0; +int DAS_apUnavailable = 0; +//fake DAS for DAS_status and DAS_status2 +int DAS_op_status = 1; +int DAS_alca_state = 0x05; +int DAS_hands_on_state = 0; +int DAS_forward_collision_warning = 0; +int DAS_cc_state = 0; +int DAS_acc_speed_limit_mph = 0; +int DAS_acc_speed_kph = 0; +int DAS_collision_warning = 0; + +//fake DAS for telemetry +int DAS_telLeftMarkerQuality = 3; //3-high, 2-medium, 1-low 0-lowest +int DAS_telRightMarkerQuality = 3; //3-high, 2-medium, 1-low 0-lowest +int DAS_telRightLaneType = 3;//0-undecided, 1-solid, 2-road edge, 3-dashed 4-double 5-botts dots 6-barrier +int DAS_telLeftLaneType = 3; //0-undecided, 1-solid, 2-road edge, 3-dashed 4-double 5-botts dots 6-barrier +int DAS_telRightLaneCrossing = 0; //0-not crossing 1-crossing +int DAS_telLeftLaneCrossing = 0; //0-not crossing 1-crossing +int DAS_telLeftMarkerColor = 1; //0-unknown, 1-white, 2-yellow, 3-blue +int DAS_telRightMarkerColor = 1; //0-unknown, 1-white, 2-yellow, 3-blue +//fake DAS for DAS_bodyControls +int DAS_turn_signal_request = 0; + +//fake DAS for DAS_steeringControls +int DAS_steeringAngle = 0x4000; +int DAS_steeringEnabled = 0; + +//fake DAS controll +int time_last_DAS_data = -1; + +//fake DAS using pedal +int DAS_usingPedal = 0; + +//fake DAS - are we in drive? +int DAS_inDrive = 0; +int DAS_inDrive_prev = 0; + +//fake DAS - last stalk data used to cancel +uint32_t DAS_lastStalkL =0x00; +uint32_t DAS_lastStalkH = 0x00; + +//fake DAS - pedal pressed (with Pedal) +int DAS_pedalPressed = 0; + +//fake DAS - DI_state spamming +uint32_t DAS_diStateL =0x00; +uint32_t DAS_diStateH = 0x00; + +//fake DAS - warning messages +int DAS_noSeatbelt = 0x00; +int DAS_canErrors = 0x00; +int DAS_plannerErrors = 0x00; +int DAS_doorOpen = 0x00; +int DAS_notInDrive = 0x00; + +static int add_tesla_crc(uint32_t MLB, uint32_t MHB , int msg_len) { + //"""Calculate CRC8 using 1D poly, FF start, FF end""" + int crc_lookup[256] = { 0x00, 0x1D, 0x3A, 0x27, 0x74, 0x69, 0x4E, 0x53, 0xE8, 0xF5, 0xD2, 0xCF, 0x9C, 0x81, 0xA6, 0xBB, + 0xCD, 0xD0, 0xF7, 0xEA, 0xB9, 0xA4, 0x83, 0x9E, 0x25, 0x38, 0x1F, 0x02, 0x51, 0x4C, 0x6B, 0x76, + 0x87, 0x9A, 0xBD, 0xA0, 0xF3, 0xEE, 0xC9, 0xD4, 0x6F, 0x72, 0x55, 0x48, 0x1B, 0x06, 0x21, 0x3C, + 0x4A, 0x57, 0x70, 0x6D, 0x3E, 0x23, 0x04, 0x19, 0xA2, 0xBF, 0x98, 0x85, 0xD6, 0xCB, 0xEC, 0xF1, + 0x13, 0x0E, 0x29, 0x34, 0x67, 0x7A, 0x5D, 0x40, 0xFB, 0xE6, 0xC1, 0xDC, 0x8F, 0x92, 0xB5, 0xA8, + 0xDE, 0xC3, 0xE4, 0xF9, 0xAA, 0xB7, 0x90, 0x8D, 0x36, 0x2B, 0x0C, 0x11, 0x42, 0x5F, 0x78, 0x65, + 0x94, 0x89, 0xAE, 0xB3, 0xE0, 0xFD, 0xDA, 0xC7, 0x7C, 0x61, 0x46, 0x5B, 0x08, 0x15, 0x32, 0x2F, + 0x59, 0x44, 0x63, 0x7E, 0x2D, 0x30, 0x17, 0x0A, 0xB1, 0xAC, 0x8B, 0x96, 0xC5, 0xD8, 0xFF, 0xE2, + 0x26, 0x3B, 0x1C, 0x01, 0x52, 0x4F, 0x68, 0x75, 0xCE, 0xD3, 0xF4, 0xE9, 0xBA, 0xA7, 0x80, 0x9D, + 0xEB, 0xF6, 0xD1, 0xCC, 0x9F, 0x82, 0xA5, 0xB8, 0x03, 0x1E, 0x39, 0x24, 0x77, 0x6A, 0x4D, 0x50, + 0xA1, 0xBC, 0x9B, 0x86, 0xD5, 0xC8, 0xEF, 0xF2, 0x49, 0x54, 0x73, 0x6E, 0x3D, 0x20, 0x07, 0x1A, + 0x6C, 0x71, 0x56, 0x4B, 0x18, 0x05, 0x22, 0x3F, 0x84, 0x99, 0xBE, 0xA3, 0xF0, 0xED, 0xCA, 0xD7, + 0x35, 0x28, 0x0F, 0x12, 0x41, 0x5C, 0x7B, 0x66, 0xDD, 0xC0, 0xE7, 0xFA, 0xA9, 0xB4, 0x93, 0x8E, + 0xF8, 0xE5, 0xC2, 0xDF, 0x8C, 0x91, 0xB6, 0xAB, 0x10, 0x0D, 0x2A, 0x37, 0x64, 0x79, 0x5E, 0x43, + 0xB2, 0xAF, 0x88, 0x95, 0xC6, 0xDB, 0xFC, 0xE1, 0x5A, 0x47, 0x60, 0x7D, 0x2E, 0x33, 0x14, 0x09, + 0x7F, 0x62, 0x45, 0x58, 0x0B, 0x16, 0x31, 0x2C, 0x97, 0x8A, 0xAD, 0xB0, 0xE3, 0xFE, 0xD9, 0xC4 }; + int crc = 0xFF; + for (int x = 0; x < msg_len; x++) { + int v = 0; + if (x <= 3) { + v = (MLB >> (x * 8)) & 0xFF; + } else { + v = (MHB >> ( (x-4) * 8)) & 0xFF; + } + crc = crc_lookup[crc ^ v]; + } + crc = crc ^ 0xFF; + return crc; +} + +static int add_tesla_cksm(CAN_FIFOMailBox_TypeDef *msg , int msg_id, int msg_len) { + int cksm = (0xFF & msg_id) + (0xFF & (msg_id >> 8)); + for (int x = 0; x < msg_len; x++) { + int v = 0; + if (x <= 3) { + v = (msg->RDLR >> (x * 8)) & 0xFF; + } else { + v = (msg->RDHR >> ( (x-4) * 8)) & 0xFF; + } + cksm = (cksm + v) & 0xFF; + } + return cksm; +} + +static int add_tesla_cksm2(uint32_t dl, uint32_t dh, int msg_id, int msg_len) { + CAN_FIFOMailBox_TypeDef to_check; + to_check.RDLR = dl; + to_check.RDHR = dh; + return add_tesla_cksm(&to_check,msg_id,msg_len); +} + +// interp function that holds extreme values +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]) + { + return xy.y[0]; + } + 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]) + { + float x0 = xy.x[i]; + float y0 = xy.y[i]; + 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; + return dy * (x - x0) / dx + y0; + } + } + // if no such point is found, then x > xy.x[size-1]. Return last point + return xy.y[size - 1]; + } +} + +static uint32_t bitShift(int value, int which_octet, int starting_bit_in_octet) { + //which octet - 1-4 + //starting_bit_in_octet 1-8 + return ( value << ((starting_bit_in_octet - 1) + (which_octet -1) * 8)); +} + +static void send_fake_message(uint32_t RIR, uint32_t RDTR,int msg_len, int msg_addr, int bus_num, uint32_t data_lo, uint32_t data_hi) { + CAN_FIFOMailBox_TypeDef to_send; + uint32_t addr_mask = 0x001FFFFF; + to_send.RIR = (msg_addr << 21) + (addr_mask & (RIR | 1)); + to_send.RDTR = (RDTR & 0xFFFFFFF0) | msg_len; + to_send.RDLR = data_lo; + to_send.RDHR = data_hi; + can_send(&to_send, bus_num); +} + +static void reset_DAS_data() { + //fake DAS variables + DAS_longC_enabled = 0; + DAS_speed_limit_kph = 0; + DAS_accel_min = 0; + DAS_accel_max = 0; + DAS_aeb_event = 0x00; + DAS_acc_state = 0x00; + DAS_jerk_min = 0x000; + DAS_jerk_max = 0x0F; + DAS_gas_to_resume = 0; + DAS_apUnavailable = 0; + DAS_op_status = 1; //unavailable + DAS_alca_state = 0x05; + DAS_hands_on_state = 0; + DAS_forward_collision_warning = 0; + DAS_cc_state = 0; + DAS_acc_speed_limit_mph = 0; + DAS_acc_speed_kph = 0; + DAS_collision_warning = 0; + DAS_telLeftMarkerQuality = 3; //3-high, 2-medium, 1-low 0-lowest + DAS_telRightMarkerQuality = 3; //3-high, 2-medium, 1-low 0-lowest + DAS_telRightLaneType = 3; //0-undecided, 1-solid, 2-road edge, 3-dashed 4-double 5-botts dots 6-barrier + DAS_telLeftLaneType = 3; //0-undecided, 1-solid, 2-road edge, 3-dashed 4-double 5-botts dots 6-barrier + DAS_telRightLaneCrossing = 0; //0-not crossing 1-crossing + DAS_telLeftLaneCrossing = 0; //0-not crossing 1-crossing + DAS_telLeftMarkerColor = 1; //0-unknown, 1-white, 2-yellow, 3-blue + DAS_telRightMarkerColor = 1; //0-unknown, 1-white, 2-yellow, 3-blue + DAS_turn_signal_request = 0; + DAS_steeringAngle = 0x4000; + DAS_steeringEnabled = 0; + DAS_usingPedal = 0; + DAS_lastStalkL =0x00; + DAS_lastStalkH = 0x00; + DAS_pedalPressed = 0; + DAS_diStateL =0x00; + DAS_diStateH = 0x00; +} + +static void do_fake_DI_state(uint32_t RIR, uint32_t RDTR) { + uint32_t MLB; + uint32_t MHB; + if (enable_das_emulation == 0) { + return; + } + if (((DAS_diStateL == 0x00) && (DAS_diStateH == 0x00)) || (DAS_cc_state != 2)) { + return; + } + MLB = (DAS_diStateL & 0xFFFF0FFF)+ 0x2000; + MHB = (DAS_diStateH & 0x00FF0E00) + ((DAS_acc_speed_limit_mph *10) & 0x1FF); + int idx = (DAS_diStateH & 0xF000 ) >> 12; + idx = ( idx + 1 ) % 16; + MHB = MHB + (idx << 12); + //cksm is different as DI_Status comes via gateway from another bus, so it's 88 or 0x058 what we need) + int cksm = add_tesla_cksm2(MLB, MHB, 0x058, 7); + MHB = MHB + (cksm << 24); + DAS_diStateL = MLB; + DAS_diStateH = MHB; + send_fake_message(RIR,RDTR,8,0x368,0,MLB,MHB); +} + +static void do_fake_DAS(uint32_t RIR, uint32_t RDTR) { + + if (enable_das_emulation == 0) { + return; + } + //check if we got data from OP in the last two seconds + if (current_car_time - time_last_DAS_data > 2) { + //no message in the last 2 seconds, reset all variables + reset_DAS_data(); + } + + fake_DAS_counter++; + fake_DAS_counter = fake_DAS_counter % 300; + uint32_t MLB; + uint32_t MHB; + + //if we never sent DAS_bootID message, send it now + if (DAS_bootID_sent == 0) { + MLB = 0x00E30055; + MHB = 0x002003BD; + send_fake_message(RIR,RDTR,8,0x639,0,MLB,MHB); + DAS_bootID_sent = 1; + } + + if (fake_DAS_counter % 2 == 0) { + //send DAS_steeringControl - 0x488 + MHB = 0x00; + MLB = ((DAS_steeringAngle >> 8) & 0x7F) + + ((DAS_steeringAngle & 0xFF) << 8) + + ((((DAS_steeringEnabled & controls_allowed & DAS_inDrive)<< 6) + DAS_steeringControl_idx) << 16); + int cksm = add_tesla_cksm2(MLB, MHB, 0x488, 3); + MLB = MLB + (cksm << 24); + send_fake_message(RIR,RDTR,4,0x488,2,MLB,MHB); + send_fake_message(RIR,RDTR,4,0x488,0,MLB,MHB); + DAS_steeringControl_idx ++; + DAS_steeringControl_idx = DAS_steeringControl_idx % 16; + } + + if (fake_DAS_counter % 10 ==7) { + //spam DI_State if we control speed as well + if (DAS_cc_state == 2) { + //do_fake_DI_state(RIR,RDTR); + } + } + + if (fake_DAS_counter % 3 == 0) { + //send DAS_object - 0x309 + //fix - 0x81,0xC0,0xF8,0xF3,0x43,0x7F,0xFD,0xF1 + //when idx==0 is lead vehicle + if (DAS_object_idx == 0) { + MLB = 0xFFFFFF00; + MHB = 0x03FFFF83; + } + if (DAS_object_idx == 1) { + MLB = 0xFFFFFF01; + MHB = 0x03FFFF83; + } + if (DAS_object_idx == 2) { + MLB = 0xFFFFFF02; + MHB = 0x03FFFF83; + } + if (DAS_object_idx == 3) { + MLB = 0xFFFFFF03; + MHB = 0x02040003; + } + //when idx == 4 we actually use 5 ; 4 is for road sign and we don't send + //05 FF FF FF FF FF FF FF + if (DAS_object_idx == 4) { + MLB = 0xFFFFFF05; + MHB = 0xFFFFFFFF; + } + send_fake_message(RIR,RDTR,8,0x309,0,MLB,MHB); + DAS_object_idx++; + DAS_object_idx = DAS_object_idx % 5; + } + + if (fake_DAS_counter % 4 == 0) { + int acc_speed_kph = 0xFFF; + int acc_state = 0x04; + int aeb_event = 0x00; + int jerk_min = 0x1FF; + int jerk_max = 0x1FF; + int accel_min = 0x1FF; + int accel_max = 0x1FF; + //send DAS_control - 0x2B9 + //when not in drive it should send FF 0F FE FF FF FF XF YY - X counter YY Checksum + if (DAS_inDrive ==1) { + if (DAS_longC_enabled > 0) { + acc_state = 0x04; + jerk_min = 0x000; + jerk_max = 0x0F; + acc_speed_kph = (int)(DAS_acc_speed_kph * 10.0); + accel_max = (int)((DAS_accel_max + 15 ) / 0.04); + accel_min = (int)((DAS_accel_min + 15 ) / 0.04); + } + MLB = (0xff & acc_speed_kph) + + (((acc_state << 4) + ((acc_speed_kph & 0xF00) >> 8)) << 8) + + ((((jerk_min << 2) & 0xFF) + aeb_event) << 16 ) + + ((((jerk_max << 3) & 0xFF) +((jerk_min >> 6) & 0x07)) << 24); + MHB = ((((accel_min << 3) & 0xFF) + ((jerk_max >> 5) & 0x07)) ) + + ((((accel_max << 4) & 0xFF) + ((accel_min >> 5) & 0x0F)) << 8) + + (((DAS_control_idx << 5) + ((accel_max >> 4) & 0x1F))<< 16); + } else { + MLB = 0xFFFE0FFF; + MHB = 0x000FFFFF + (DAS_control_idx << 21); + } + int cksm = add_tesla_cksm2(MLB, MHB, 0x2B9, 7); + MHB = MHB + (cksm << 24); + send_fake_message(RIR,RDTR,8,0x2B9,0,MLB,MHB); + DAS_control_idx++; + DAS_control_idx = DAS_control_idx % 8; + } + if (fake_DAS_counter % 4 == 2) { + //send DAS_pscControl - 0x219 + // 0x90 + DAS_pscControl_idx,0x00,0x00,0x00,0x00,0x00,0x00,0x00 + MLB = 0x90 + DAS_pscControl_idx; + MHB = 0x00; + int cksm = add_tesla_cksm2(MLB, MHB, 0x219, 2); + MLB = MLB + (cksm << 16); + send_fake_message(RIR,RDTR,3,0x219,0,MLB,MHB); + DAS_pscControl_idx++; + DAS_pscControl_idx = DAS_pscControl_idx % 16; + + //send DAS_telemetryFurniture - 0x3B1 + //NOT SENDING FOR NOW + } + + if (fake_DAS_counter % 4 == 3) { + //send DAS_telemetryPeriodic - 0x379 + //static for now + switch (DAS_telemetryPeriodic_idx2) { + case 0: //Y0 3A 83 D3 E8 07 6D 11 + MLB = 0xd3833a00; + MHB = 0x116d07e8; + break; + case 1: //Y1 E1 D7 E6 FB BF 7D 00 + MLB = 0xE6D7E101; + MHB = 0x007DBFFB; + break; + case 2: //42 49 F4 27 FF D1 7D 00 + MLB = 0x27F44902; + MHB = 0x007DD1FF; + break; + case 3: //43 D7 AB A7 FF F3 FE C0 + MLB = 0xA7ABD703; + MHB = 0xC0FEF3FF; + break; + case 4: //44 7B FB B7 F1 33 83 FF + MLB = 0xB7FB7B04; + MHB = 0xFF8333F1; + break; + case 5: //45 EA C7 5F F7 27 83 40 + MLB = 0x5Fc7EA05; + MHB = 0x408327F7; + break; + case 6: //46 01 48 00 00 00 00 00 + MLB = 0x00480106; + MHB = 0x00000000; + break; + case 7: //47 01 48 00 00 00 00 00 + MLB = 0x00480107; + MHB = 0x00000000; + break; + case 8: //48 00 00 00 00 00 00 00 + MLB = 0x00000008; + MHB = 0x00000000; + break; + default: //49 FA 37 00 00 00 00 08 + MLB = 0x0037FA09; + MHB = 0x08000000; + break; + } + MLB = MLB + (DAS_telemetryPeriodic_idx2 << 5); + send_fake_message(RIR,RDTR,1,0x379,0,MLB,MHB); + DAS_telemetryPeriodic_idx2++; + DAS_telemetryPeriodic_idx2 = DAS_telemetryPeriodic_idx2 % 10; + if (DAS_telemetryPeriodic_idx2 == 0) { + DAS_telemetryPeriodic_idx1 += 1; + DAS_telemetryPeriodic_idx1 = DAS_telemetryPeriodic_idx1 % 8; + } + + } + + if (fake_DAS_counter % 6 == 0) { + //send DAS_integratedSafetyFront - 0x299 + //NOT SENDING FOR NOW + + } + + if (fake_DAS_counter % 10 == 3) { + //send DAS_lanes - 0x239 + //for now fixed 0x33,0xC8,0xF0,0x7F,0x70,0x70,0x33,(idx << 4)+0x0F + MLB = 0x7FF0C833; + MHB = 0x0F337070 + (DAS_lanes_idx << 28); + send_fake_message(RIR,RDTR,8,0x239,0,MLB,MHB); + DAS_lanes_idx ++; + DAS_lanes_idx = DAS_lanes_idx % 16; + } + + if (fake_DAS_counter % 10 == 4) { + //send DAS_telemetry - 0x3A9 + //no counter - 00 00 00 00 00 00 00 00 + MLB = 0x00 + ((DAS_telLeftLaneType + (DAS_telRightLaneType << 3) + (DAS_telLeftMarkerQuality << 6)) << 8 ) + + ((DAS_telRightMarkerQuality +(DAS_telLeftMarkerColor << 2) + + (DAS_telRightMarkerColor << 4) + (DAS_telLeftLaneCrossing << 6) +(DAS_telRightLaneCrossing <<6)) << 16); + MHB =0x00; + send_fake_message(RIR,RDTR,8,0x3A9,0,MLB,MHB); + } + + if (fake_DAS_counter % 10 == 5) { + //send DAS_chNm - 0x409 + //no counter + MLB = 0x00; + MHB = 0x00; + send_fake_message(RIR,RDTR,1,0x409,0,MLB,MHB); + + //send DAS_telemetryEvent - 0x3D9 + //NOT SENDING FOR NOW + + //send DAS_visualDebug - 0x249 + //no counter - 8A 06 21 1F 00 00 00 00 + MLB = 0x1F21068A; + MHB =0x00; + send_fake_message(RIR,RDTR,8,0x249,0,MLB,MHB); + + } + + + if (fake_DAS_counter % 50 == 0) { + //send DAS_status - 0x399 + int sl = (int)(DAS_speed_limit_kph / 5); + MLB = DAS_op_status + (sl << 8) + (((DAS_collision_warning << 6) + sl) << 16); + MHB = (DAS_cc_state << 3) + + (((DAS_hands_on_state << 2) + ((DAS_alca_state & 0x03) << 6)) << 8) + + ((( DAS_status_idx << 4) + (DAS_alca_state >> 2)) << 16); + int cksm = add_tesla_cksm2(MLB, MHB, 0x399, 7); + MHB = MHB + (cksm << 24); + send_fake_message(RIR,RDTR,8,0x399,0,MLB,MHB); + DAS_status_idx ++; + DAS_status_idx = DAS_status_idx % 16; + + //send DAS_status2 - 0x389 + int sl2 = (int)(DAS_acc_speed_limit_mph / 0.2); + if (sl2 == 0) { + sl2 = 0x3FF; + } + MLB = sl2; + MHB = 0x8017 + (DAS_status2_idx << 20) + (DAS_forward_collision_warning << 16); + cksm = add_tesla_cksm2(MLB, MHB, 0x389, 7); + MHB = MHB + (cksm << 24); + send_fake_message(RIR,RDTR,8,0x389,0,MLB,MHB); + DAS_status2_idx ++; + DAS_status2_idx = DAS_status2_idx % 16; + } + if (fake_DAS_counter % 50 == 9) { + //send DAS_bodyControls - 0x3E9 + //0xf1,0x0c + turn_signal_request,0x00,0x00,0x00,0x00,(idx << 4) + MLB = 0x00000CF1 + (DAS_turn_signal_request << 8); + MHB = 0x00 + (DAS_bodyControls_idx << 20); + int cksm = add_tesla_cksm2(MLB, MHB, 0x3E9, 7); + MHB = MHB + (cksm << 24); + send_fake_message(RIR,RDTR,8,0x3E9,0,MLB,MHB); + DAS_bodyControls_idx ++; + DAS_bodyControls_idx = DAS_bodyControls_idx % 16; + } + + if (fake_DAS_counter % 100 == 0) { + //send DAS_dtcMatrix - 0x669 + //NOT SENDING FOR NOW + + //send DAS_info - 0x539 + switch (DAS_info_idx) { + case 0: + MLB = 0x0003010a; + MHB = 0x004e0000; + break; + case 1: + MLB = 0x0102000b; + MHB = 0x00000001; + break; + case 2: + MLB = 0x0000000d; + MHB = 0x3eac8e5b; + break; + case 3: + MLB = 0xc9060010; + MHB = 0x000021b9; + break; + case 4: + MLB = 0x00000011; + MHB = 0x00000000; + break; + case 5: + MLB = 0x34577f12; + MHB = 0x0b89706f; + break; + case 6: + MLB = 0xffff0113; + MHB = 0x0000fcff; + break; + case 7: + MLB = 0x00000514; + MHB = 0xc309fce5; + break; + case 8: + MLB = 0xe7700017; + MHB = 0x000091c4; + break; + default: + MLB = 0xb2d50018; + MHB = 0x0000786c; + break; + } + send_fake_message(RIR,RDTR,8,0x539,0,MLB,MHB); + DAS_info_idx ++; + DAS_info_idx = DAS_info_idx % 10; + } + if (fake_DAS_counter % 100 == 45) { + //send DAS_warningMatrix0 - 0x329 + MLB = 0x00 + bitShift(DAS_canErrors,4,7); + MHB = 0x00 + bitShift(DAS_notInDrive,2,2); + send_fake_message(RIR,RDTR,8,0x329,0,MLB,MHB); + DAS_warningMatrix0_idx ++; + DAS_warningMatrix0_idx = DAS_warningMatrix0_idx % 16; + + //send DAS_warningMatrix1 - 0x369 + MLB = 0x00; + MHB = 0x00; + send_fake_message(RIR,RDTR,8,0x369,0,MLB,MHB); + DAS_warningMatrix1_idx ++; + DAS_warningMatrix1_idx = DAS_warningMatrix1_idx % 16; + + //send DAS_warningMatrix3 - 0x349 + int ovr = 0; + int lcAborting = 0; + int lcUnavailableSpeed = 0; + if (DAS_alca_state == 0x14) { + lcAborting == 1; + } + if (DAS_alca_state == 0x05) { + lcUnavailableSpeed == 1; + } + if ((DAS_cc_state == 2) && (DAS_pedalPressed > 10)) { + ovr = 1; + } + MLB = 0x00 + bitShift(DAS_gas_to_resume,1,2) + bitShift(DAS_apUnavailable,2,6) + bitShift(ovr,3,8) + + bitShift(lcAborting,2,1) + bitShift(lcUnavailableSpeed,4,3) + bitShift(DAS_noSeatbelt,3,3) + bitShift(DAS_plannerErrors,4,6); + MHB = 0x00; + send_fake_message(RIR,RDTR,8,0x349,0,MLB,MHB); + DAS_warningMatrix3_idx ++; + DAS_warningMatrix3_idx = DAS_warningMatrix3_idx % 16; + } +} + +static void do_EPB_epasControl(uint32_t RIR, uint32_t RDTR) { + uint32_t MLB; + uint32_t MHB; + MLB = 0x01 + (EPB_epasControl_idx << 8); + MHB = 0x00; + int cksm = add_tesla_cksm2(MLB, MHB, 0x214, 2); + MLB = MLB + (cksm << 16); + send_fake_message(RIR,RDTR,3,0x214,2,MLB,MHB); + EPB_epasControl_idx++; + EPB_epasControl_idx = EPB_epasControl_idx % 16; +} + +static void do_fake_stalk_cancel(uint32_t RIR, uint32_t RDTR) { + uint32_t MLB; + uint32_t MHB; + if ((DAS_lastStalkL == 0x00) && (DAS_lastStalkH == 0x00)) { + return; + } + MLB = (DAS_lastStalkL & 0xFFFFFFC0) + 0x01; + MHB = (DAS_lastStalkH & 0x000FFFFF); + int idx = (DAS_lastStalkH & 0xF00000 ) >> 20; + idx = ( idx + 1 ) % 16; + MHB = MHB + (idx << 20); + int crc = add_tesla_crc(MLB, MHB,7); + MHB = MHB + (crc << 24); + DAS_lastStalkH = MHB; + send_fake_message(RIR,RDTR,8,0x45,0,MLB,MHB); +} -void set_gmlan_digital_output(int to_set); -void reset_gmlan_switch_timeout(void); -void gmlan_switch_init(int timeout_enable); static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - set_gmlan_digital_output(0); // #define GMLAN_HIGH 0 + 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 + uint32_t ts = TIM2->CNT; - //int bus_number = (to_push->RDTR >> 4) & 0xFF; + int bus_number = (to_push->RDTR >> 4) & 0xFF; uint32_t addr; + if (to_push->RIR & 4) { // Extended @@ -67,76 +719,308 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) addr = to_push->RIR >> 21; } + //let's see if the pedal was pressed + if ((addr == 0x552) && (bus_number == 2)) { + //m1 = 0.050796813 + //m2 = 0.101593626 + //d = -22.85856576 + DAS_pedalPressed = (int)((((to_push->RDLR & 0xFF00) >> 8) + ((to_push->RDLR & 0xFF) << 8)) * 0.050796813 -22.85856576); + } + + //we use 0x108 at 100Hz to detect timing of messages sent by our fake DAS and EPB + if (addr == 0x108) { + if (fake_DAS_counter % 10 == 5) { + do_EPB_epasControl(to_push->RIR,to_push->RDTR); + } + do_fake_DAS(to_push->RIR,to_push->RDTR); + return; + } + + //see if cruise is enabled [Enabled, standstill or Override] and cancel if using pedal + if (addr == 0x368) { + //first save values for spamming + DAS_diStateL = to_push->RDLR; + DAS_diStateH = to_push->RDHR; + int acc_state = ((to_push->RDLR & 0xF000) >> 12); + if ((DAS_usingPedal == 1) && ( acc_state >= 2) && ( acc_state <= 4)) { + do_fake_stalk_cancel(to_push->RIR, to_push->RDTR); + } + if (DAS_cc_state == 2) { + //do_fake_DI_state(to_push->RIR,to_push->RDTR); + } + + } + + // 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; + } + + //looking for radar messages; + if ((addr == 0x300) && (bus_number ==1)) + { + uint32_t ts = TIM2->CNT; + uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_last_radar_signal); + if (tesla_radar_status == 1) { + tesla_radar_status = 2; + puts("Tesla Radar Active! \n"); + tesla_last_radar_signal = ts; + } else + if ((ts_elapsed > TESLA_RADAR_TIMEOUT) && (tesla_radar_status > 0)) { + tesla_radar_status = 0; + puts("Tesla Radar Inactive! (timeout 1) \n"); + } else + if ((ts_elapsed <= TESLA_RADAR_TIMEOUT) && (tesla_radar_status == 2)) { + tesla_last_radar_signal = ts; + } + } + + //0x631 is sent by radar to initiate the sync + if ((addr == 0x631) && (bus_number == 1)) + { + uint32_t ts = TIM2->CNT; + uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_last_radar_signal); + if (tesla_radar_status == 0) { + tesla_radar_status = 1; + tesla_last_radar_signal = ts; + puts("Tesla Radar Initializing... \n"); + } else + if ((ts_elapsed > TESLA_RADAR_TIMEOUT) && (tesla_radar_status > 0)) { + tesla_radar_status = 0; + puts("Tesla Radar Inactive! (timeout 2) \n"); + } else + if ((ts_elapsed <= TESLA_RADAR_TIMEOUT) && (tesla_radar_status > 0)) { + tesla_last_radar_signal = ts; + } + } + if (addr == 0x45) { + //first save for future use + DAS_lastStalkL = to_push->RDLR; + DAS_lastStalkH = to_push->RDHR; // 6 bits starting at position 0 - int lever_position = (to_push->RDLR & 0x3F); - if (lever_position == 2) + int ap_lever_position = (to_push->RDLR & 0x3F); + if (ap_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; //} + time_at_last_stalk_pull = current_car_time; } - else if (lever_position == 1) + else if (ap_lever_position == 1) { // push towards the back // deactivate openpilot controls_allowed = 0; } + //if using pedal, send a cancel immediately to cancel the pedal + if ((DAS_usingPedal == 1) && (ap_lever_position > 1)) { + do_fake_stalk_cancel(to_push->RIR, to_push->RDTR); + } + /* <-- revB giraffe GPIO */ + int turn_signal_lever = (to_push->RDLR >> 16) & 0x3; //TurnIndLvr_Stat : 16|2@1+ + int stw_menu_button = (to_push->RDHR >> 5) & 0x1; //StW_Sw05_Psd : 37|1@1+ + high_beam_lever_state = (to_push->RDLR >> 18) & 0x3; //SG_ HiBmLvr_Stat : 18|2@1+ + + //TurnIndLvr_Stat 3 "SNA" 2 "RIGHT" 1 "LEFT" 0 "IDLE" ; + if (turn_signal_lever == 1) + { + //Left turn signal is on, turn on output pin 3 + set_uja1023_output_bits(1 << 3); + //puts(" Left turn on!\n"); + } + else + { + clear_uja1023_output_bits(1 << 3); + } + if (turn_signal_lever == 2) + { + //Right turn signal is on, turn on output pin 4 + set_uja1023_output_bits(1 << 4); + //puts(" Right turn on!\n"); + } + else + { + clear_uja1023_output_bits(1 << 4); + } + + if (stw_menu_button == 1) + { + //menu button is pushed, if it wasn't last time, set the initial timestamp + if (stw_menu_btn_state_last == 0) + { + stw_menu_btn_state_last = 1; + stw_menu_btn_pressed_ts = ts; + } + else + { + uint32_t stw_ts_elapsed = get_ts_elapsed(ts, stw_menu_btn_pressed_ts); + if (stw_ts_elapsed > STW_MENU_BTN_HOLD_INTERVAL) + { + //user held the button, do stuff! + if (stw_menu_current_output_state == 0 && stw_menu_output_flag == 0) + { + stw_menu_output_flag = 1; + stw_menu_current_output_state = 1; + //set_uja1023_output_bits(1 << 5); + //puts("Menu Button held, setting output 5 HIGH\n"); + } + else if (stw_menu_current_output_state == 1 && stw_menu_output_flag == 0) + { + stw_menu_output_flag = 1; + stw_menu_current_output_state = 0; + //clear_uja1023_output_bits(1 << 5); + //puts("Menu Button held, setting output 5 LOW\n"); + } + } //held + } + } //stw menu button pressed + else if (stw_menu_button == 0) + { + stw_menu_output_flag = 0; + stw_menu_btn_state_last = 0; + } + /* revB giraffe GPIO --> */ } // Detect drive rail on (ignition) (start recording) - if (addr == 0x348) + if ((addr == 0x348) && (bus_number == 0)) { // GTW_status int drive_rail_on = (to_push->RDLR & 0x0001); tesla_ignition_started = drive_rail_on == 1; + + //ALSO use this for radar timeout, this message is always on + uint32_t ts = TIM2->CNT; + uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_last_radar_signal); + if ((ts_elapsed > TESLA_RADAR_TIMEOUT) && (tesla_radar_status > 0)) { + tesla_radar_status = 0; + puts("Tesla Radar Inactive! (timeout 3) \n"); + } } // exit controls on brake press // DI_torque2::DI_brakePedal 0x118 - if (addr == 0x118) + + /* revB giraffe GPIO --> */ + if ((addr == 0x118) && (bus_number == 0)) { - // 1 bit at position 16 - if (((to_push->RDLR & 0x8000)) >> 15 == 1) + int drive_state = (to_push->RDLR >> 12) & 0x7; //DI_gear : 12|3@1+ + int brake_pressed = (to_push->RDLR & 0x8000) >> 15; + int tesla_speed_mph = ((((((to_push->RDLR >> 24) & 0x0F) << 8) + (( to_push->RDLR >> 16) & 0xFF)) * 0.05 -25)); + + //if the car goes into reverse, set UJA1023 output pin 5 to high. If Drive, set pin 1 high + //DI_gear 7 "DI_GEAR_SNA" 4 "DI_GEAR_D" 3 "DI_GEAR_N" 2 "DI_GEAR_R" 1 "DI_GEAR_P" 0 "DI_GEAR_INVALID" ; + //UJA1023 pin0 is our output to the camera switcher + + if (drive_state == 2) + { + //reverse_state = 1; + set_uja1023_output_bits(1 << 5); + + //if we're in reverse, we always want the rear camera up: + set_uja1023_output_bits(1 << 0); //show rear camera + //puts(" Got Reverse\n"); + + } + else + { + //reverse_state = 0; + clear_uja1023_output_bits(1 << 5); + + //if we're in not in reverse and button state is 0, set output low (show front camera) + if (stw_menu_current_output_state == 0) + { + clear_uja1023_output_bits(1 << 0); //show front camera + } + + //if we're not in reverse and button state is 1, set the output high (show the rear camera) + else + { + set_uja1023_output_bits(1 << 0); //show rear camera + } + } + + if (drive_state == 4) + { + set_uja1023_output_bits(1 << 1); + //puts(" Got Drive\n"); + } + else + { + clear_uja1023_output_bits(1 << 1); + } + + if (brake_pressed == 1) { //disable break cancel by commenting line below - controls_allowed = 0; + //controls_allowed = 0; + + set_uja1023_output_bits(1 << 2); + //puts(" Brake on!\n"); + tesla_ts_brakelight_on_last = ts; } - //get vehicle speed in m/s. Tesla gives MPH - tesla_speed = ((((((to_push->RDLR >> 24) & 0x0F) << 8) + ((to_push->RDLR >> 16) & 0xFF)) * 0.05 - 25) * 1.609 / 3.6); + else + { + uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_ts_brakelight_on_last); + if (ts_elapsed > BRAKELIGHT_CLEAR_INTERVAL) + { + clear_uja1023_output_bits(1 << 2); + //puts(" Brakelight off!\n"); + } + } + /* revB giraffe GPIO --> */ + + //get vehicle speed in m/2. Tesla gives MPH + tesla_speed = (tesla_speed_mph*1.609/3.6); if (tesla_speed < 0) { tesla_speed = 0; } + DAS_inDrive_prev = DAS_inDrive; + if (((to_push->RDLR & 0x7000 ) >> 12) == 4) { + DAS_inDrive = 1; + } else { + DAS_inDrive = 0; + } + if ((DAS_inDrive == 0) && (DAS_inDrive_prev == 1)) { + reset_DAS_data(); + } } // exit controls on EPAS error // EPAS_sysStatus::EPAS_eacStatus 0x370 - if (addr == 0x370) + if ((addr == 0x370) && (bus_number == 1)) { // if EPAS_eacStatus is not 1 or 2, disable control eac_status = ((to_push->RDHR >> 21)) & 0x7; // 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 steering allowed + // 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"); + puts("EPAS error! \n"); } } //get latest steering wheel angle - if (addr == 0x00E) + if ((addr == 0x00E) && (bus_number == 0)) { - float angle_meas_now = (int)((((to_push->RDLR & 0x3F) << 8) + ((to_push->RDLR >> 8) & 0xFF)) * 0.1 - 819.2); + 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, 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 - float rt_delta_angle_up = interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, tesla_speed) * 25. + 1.; - float rt_delta_angle_down = interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, tesla_speed) * 25. + 1.; - float highest_rt_angle = tesla_rt_angle_last + (tesla_rt_angle_last > 0 ? rt_delta_angle_up : rt_delta_angle_down); - float lowest_rt_angle = tesla_rt_angle_last - (tesla_rt_angle_last > 0 ? rt_delta_angle_down : rt_delta_angle_up); + 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); if ((ts_elapsed > TESLA_RT_INTERVAL) || (controls_allowed && !tesla_controls_allowed_last)) { @@ -144,21 +1028,75 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) tesla_ts_angle_last = ts; } + // update array of samples + update_sample(&tesla_angle_meas, angle_meas_now); + // check for violation; - if (fmax_limit_check(angle_meas_now, highest_rt_angle, lowest_rt_angle)) + 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) - controls_allowed = 0; - //puts("WARN: RT Angle - No steer allowed! \n"); + steer_allowed = 0; + puts("WARN: RT Angle - No steer allowed! \n"); } else { - controls_allowed = 1; + steer_allowed = 1; } tesla_controls_allowed_last = controls_allowed; } + + /* <-- revB giraffe GPIO */ + //BO_ 1001 DAS_bodyControls: 8 XXX + if (addr == 0x3e9) + { + int high_beam_decision = (to_push->RDLR >> 10) & 0x3; //DAS_highLowBeamDecision : 10|2@1+ + // highLowBeamDecision: + //0: Undecided (Car off) + //1: Off + //2: On + //3: Auto High Beam is disabled + //VAL_ 69 HiBmLvr_Stat 3 "SNA" 2 "HIBM_FLSH_ON_PSD" 1 "HIBM_ON_PSD" 0 "IDLE" ; + + //If the lever is in either high beam position and auto high beam is off or indicates highs should be on + if ((high_beam_decision == 3 && (high_beam_lever_state == 2 || high_beam_lever_state == 1)) + || (high_beam_decision == 2 && (high_beam_lever_state == 2 || high_beam_lever_state == 1))) + { + //high beams are on. Set the output 6 high + set_uja1023_output_bits(1 << 6); + //puts("High Beam on!\n"); + } //high beams on! + else + { + //high beams are off. Set the output 6 low + clear_uja1023_output_bits(1 << 6); + //puts("High Beam off!\n"); + } //high beams off + } //DAS_bodyControls + + //BO_ 872 DI_state: 8 DI + if (addr == 0x368) + { + int regen_brake_light = (to_push->RDLR >> 8) & 0x1; //DI_regenLight : 8|1@1+ + //if the car's brake lights are on, set pin 2 to high + if (regen_brake_light == 1) + { + set_uja1023_output_bits(1 << 2); + //puts(" Regen Brake Light on!\n"); + tesla_ts_brakelight_on_last = ts; + } + else + { + uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_ts_brakelight_on_last); + if (ts_elapsed > BRAKELIGHT_CLEAR_INTERVAL) + { + clear_uja1023_output_bits(1 << 2); + //puts(" Brakelight off!\n"); + } + } + } + /* revB giraffe GPIO --> */ } // all commands: gas/regen, friction brake and steering @@ -171,51 +1109,87 @@ static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { uint32_t addr; - float angle_raw; - float desired_angle; + 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) - { - angle_raw = ((to_send->RDLR & 0x7F) << 8) + ((to_send->RDLR & 0xFF00) >> 8); - desired_angle = angle_raw * 0.1 - 1638.35; + + //capture message for fake DAS warning + if (addr == 0x554) { + int b0 = (to_send->RDLR & 0xFF); + DAS_noSeatbelt = ((b0 >> 4) & 0x01); + DAS_canErrors = ((b0 >> 3) & 0x01); + DAS_plannerErrors = ((b0 >> 2) & 0x01); + DAS_doorOpen = ((b0 >> 1) & 0x01); + DAS_notInDrive = ((b0 >> 0) & 0x01); + enable_das_emulation = ((b0 >> 5) & 0x01); + enable_radar_emulation = ((b0 >> 6) & 0x01); + return false; + } + //capture message for fake DAS and parse + if (addr == 0x553) { + int b0 = (to_send->RDLR & 0xFF); + int b1 = ((to_send->RDLR >> 8) & 0xFF); + int b2 = ((to_send->RDLR >> 16) & 0xFF); + int b3 = ((to_send->RDLR >> 24) & 0xFF); + int b4 = (to_send->RDHR & 0xFF); + int b5 = ((to_send->RDHR >> 8) & 0xFF); + int b6 = ((to_send->RDHR >> 16) & 0xFF); + int b7 = ((to_send->RDHR >> 24) & 0xFF); + + DAS_acc_speed_kph = b1; + DAS_acc_speed_limit_mph = b4; + DAS_longC_enabled = ((b0 & 0x80) >> 7); + DAS_gas_to_resume = ((b0 & 0x40) >> 6); + DAS_apUnavailable = ((b0 & 0x20) >> 5); + DAS_collision_warning = ((b0 & 0x10) >> 4); + DAS_op_status = (b0 & 0x0F); + DAS_turn_signal_request = ((b2 & 0xC0) >> 6); + DAS_forward_collision_warning = ((b2 & 0x30) >> 4); + DAS_hands_on_state = (b2 & 0x0F); + DAS_cc_state = ((b3 & 0xC0)>>6); + DAS_usingPedal = ((b3 & 0x20) >> 5); + DAS_alca_state = (b3 & 0x1F); + DAS_speed_limit_kph = b5; + time_last_DAS_data = current_car_time; + DAS_steeringAngle = ((b7 << 8) + b6) & 0x7FFF; + DAS_steeringEnabled = (b7 >> 7); + + desired_angle = DAS_steeringAngle * 0.1 - 1638.35; int16_t violation = 0; - int st_enabled = (to_send->RDLR & 0x400000) >> 22; - if (st_enabled == 0) { + if (DAS_steeringEnabled == 0) { //steering is not enabled, do not check angles and do send tesla_desired_angle_last = desired_angle; - return true; - } - + } else if (controls_allowed) { // add 1 to not false trigger the violation - float delta_angle_up = interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, tesla_speed) + 1.; - float delta_angle_down = interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, tesla_speed) + 1.; - float highest_desired_angle = tesla_desired_angle_last + (tesla_desired_angle_last > 0 ? delta_angle_up : delta_angle_down); - float lowest_desired_angle = tesla_desired_angle_last - (tesla_desired_angle_last > 0 ? delta_angle_down : delta_angle_up); - float TESLA_MAX_ANGLE = interpolate(TESLA_LOOKUP_MAX_ANGLE, tesla_speed) + 1.; - - //check for max angles - violation |= fmax_limit_check(desired_angle, TESLA_MAX_ANGLE, -TESLA_MAX_ANGLE); + 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.); - //check for angle delta changes - violation |= fmax_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle); - - if (violation) + if (max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle)) { + violation = 1; controls_allowed = 0; - return false; + 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"); } - tesla_desired_angle_last = desired_angle; - return true; } + tesla_desired_angle_last = desired_angle; + return false; } + return true; } @@ -237,6 +1211,201 @@ static int tesla_ign_hook() return tesla_ignition_started; } +static void tesla_fwd_to_radar_as_is(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { + if (enable_radar_emulation == 0) { + return; + } + CAN_FIFOMailBox_TypeDef to_send; + to_send.RIR = to_fwd->RIR | 1; // TXRQ + to_send.RDTR = to_fwd->RDTR; + to_send.RDLR = to_fwd->RDLR; + to_send.RDHR = to_fwd->RDHR; + can_send(&to_send, bus_num); +} + +static uint32_t radar_VIN_char(int pos, int shift) { + return (((int)radar_VIN[pos]) << (shift * 8)); +} + + +static void tesla_fwd_to_radar_modded(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { + if (enable_radar_emulation == 0) { + return; + } + int32_t addr = to_fwd->RIR >> 21; + CAN_FIFOMailBox_TypeDef to_send; + to_send.RIR = to_fwd->RIR | 1; // TXRQ + to_send.RDTR = to_fwd->RDTR; + to_send.RDLR = to_fwd->RDLR; + to_send.RDHR = to_fwd->RDHR; + uint32_t addr_mask = 0x001FFFFF; + //now modd + if (addr == 0x405 ) + { + to_send.RIR = (0x2B9 << 21) + (addr_mask & (to_fwd->RIR | 1)); + if (((to_send.RDLR & 0x10) == 0x10) && (sizeof(radar_VIN) == 17)) + { + int rec = to_send.RDLR & 0xFF; + if (rec == 0x10) { + to_send.RDLR = 0x00000000 | rec; + to_send.RDHR = 0x00000000; + to_send.RDHR = radar_VIN_char(0,1) | radar_VIN_char(1,2) | radar_VIN_char(2,3); + } + if (rec == 0x11) { + to_send.RDLR = 0x00000000 | rec; + to_send.RDLR = radar_VIN_char(3,1) | radar_VIN_char(4,2) | radar_VIN_char(5,3); + to_send.RDHR = 0x00000000; + to_send.RDHR = radar_VIN_char(6,0) | radar_VIN_char(7,1) | radar_VIN_char(8,2) | radar_VIN_char(9,3); + } + if (rec == 0x12) { + to_send.RDLR = 0x00000000 | rec; + to_send.RDLR = radar_VIN_char(10,1) | radar_VIN_char(11,2) | radar_VIN_char(12,3); + to_send.RDHR = 0x00000000; + to_send.RDHR = radar_VIN_char(13,0) | radar_VIN_char(14,1) | radar_VIN_char(15,2) | radar_VIN_char(16,3); + } + } + can_send(&to_send, bus_num); + + return; + } + if (addr == 0x398 ) + { + //change frontradarHW = 1 and dashw = 1 + //SG_ GTW_dasHw : 7|2@0+ (1,0) [0|0] "" NEO + //SG_ GTW_parkAssistInstalled : 11|2@0+ (1,0) [0|0] "" NEO + to_send.RDHR = to_send.RDHR | 0x100; + //resend on CAN 0 first + to_send.RIR = (to_fwd->RIR | 1); + can_send(&to_send,0); + + + to_send.RDLR = to_send.RDLR & 0xFFFFF33F; + to_send.RDLR = to_send.RDLR | 0x440; + // change the autopilot to 1 + to_send.RDHR = to_fwd->RDHR & 0xCFFFFFFF; + to_send.RDHR = to_send.RDHR | 0x10000000; + + if ((sizeof(radar_VIN) == 17) && ((int)(radar_VIN[7]) == 0x32)) { + //also change to AWD if needed (most likely) if manual VIN and if position 8 of VIN is a 2 (dual motor) + to_send.RDLR = to_send.RDLR | 0x08; + } + //now change address and send to radar + to_send.RIR = (0x2A9 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num); + + return; + } + if (addr == 0x00E ) + { + to_send.RIR = (0x199 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num); + + return; + } + if (addr == 0x20A ) + { + to_send.RIR = (0x159 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num); + + return; + } + if (addr == 0x115 ) + { + + int counter = ((to_fwd->RDHR & 0xF0) >> 4 ) & 0x0F; + + to_send.RIR = (0x129 << 21) + (addr_mask & (to_fwd->RIR | 1)); + int cksm = (0x16 + (counter << 4)) & 0xFF; + can_send(&to_send, bus_num); + + //we don't get 0x148 DI_espControl so send as 0x1A9 on CAN1 and also as 0x148 on CAN0 + to_send.RDTR = (to_fwd->RDTR & 0xFFFFFFF0) | 0x05; + to_send.RIR = (0x148 << 21) + (addr_mask & (to_fwd->RIR | 1)); + to_send.RDLR = 0x000C0000 | (counter << 28); + cksm = (0x38 + 0x0C + (counter << 4)) & 0xFF; + to_send.RDHR = cksm; + //can_send(&to_send, 0); + + to_send.RIR = (0x1A9 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num); + + return; + } + + if (addr == 0x145) + { + to_send.RIR = (0x149 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num); + + return; + } + + if (addr == 0x118 ) + { + to_send.RIR = (0x119 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num); + + //we don't get 0x175 ESP_wheelSpeeds so send as 0x169 on CAN1 and also as 0x175 on CAN0 + int counter = to_fwd->RDHR & 0x0F; + to_send.RIR = (0x169 << 21) + (addr_mask & (to_fwd->RIR | 1)); + to_send.RDTR = (to_fwd->RDTR & 0xFFFFFFF0) | 0x08; + int32_t speed_kph = (((0xFFF0000 & to_send.RDLR) >> 16) * 0.05 -25) * 1.609; + if (speed_kph < 0) { + speed_kph = 0; + } + //speed_kph = 20; //force it at 20 kph for debug + speed_kph = (int)(speed_kph/0.04) & 0x1FFF; + to_send.RDLR = (speed_kph | (speed_kph << 13) | (speed_kph << 26)) & 0xFFFFFFFF; + to_send.RDHR = ((speed_kph >> 6) | (speed_kph << 7) | (counter << 20)) & 0x00FFFFFF; + int cksm = 0x76; + cksm = (cksm + (to_send.RDLR & 0xFF) + ((to_send.RDLR >> 8) & 0xFF) + ((to_send.RDLR >> 16) & 0xFF) + ((to_send.RDLR >> 24) & 0xFF)) & 0xFF; + cksm = (cksm + (to_send.RDHR & 0xFF) + ((to_send.RDHR >> 8) & 0xFF) + ((to_send.RDHR >> 16) & 0xFF) + ((to_send.RDHR >> 24) & 0xFF)) & 0xFF; + to_send.RDHR = to_send.RDHR | (cksm << 24); + can_send(&to_send, bus_num); + + to_send.RIR = (0x175 << 21) + (addr_mask & (to_fwd->RIR | 1)); + //can_send(&to_send, 0); + + return; + } + if (addr == 0x108 ) + { + to_send.RIR = (0x109 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num); + + return; + } + if (addr == 0x308 ) + { + to_send.RIR = (0x209 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num); + + return; + } + if (addr == 0x45 ) + { + to_send.RIR = (0x219 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num); + + return; + } + if (addr == 0x148 ) + { + to_send.RIR = (0x1A9 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num); + + return; + } + if (addr == 0x30A) + { + to_send.RIR = (0x2D9 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num); + + return; + } + +} + static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { @@ -245,6 +1414,26 @@ static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) if (bus_num == 0) { + //check all messages we need to also send to radar, moddified, after we receive 0x631 from radar + //148 does not exist, we use 115 at the same frequency to trigger and pass static vals + //175 does not exist, we use 118 at the same frequency to trigger and pass vehicle speed + if ((tesla_radar_status > 0 ) && (enable_radar_emulation == 1) && ((addr == 0x20A ) || (addr == 0x118 ) || (addr == 0x108 ) || + (addr == 0x115 ) || (addr == 0x148 ) || (addr == 0x145))) + { + tesla_fwd_to_radar_modded(1, to_fwd); + } + + //check all messages we need to also send to radar, moddified, all the time + if (((addr == 0xE ) || (addr == 0x308 ) || (addr == 0x45 ) || (addr == 0x398 ) || + (addr == 0x405 ) || (addr == 0x30A)) && (enable_radar_emulation == 1)) { + tesla_fwd_to_radar_modded(1, to_fwd); + } + + //forward to radar unmodded the UDS messages 0x641 + if (addr == 0x641 ) { + tesla_fwd_to_radar_as_is(1, to_fwd); + } + // change inhibit of GTW_epasControl if (addr == 0x101) { @@ -258,23 +1447,39 @@ static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) // remove EPB_epasControl if (addr == 0x214) { - return false; + return -1; } return 2; // Custom EPAS bus } + + if (bus_num == 1) { + //send radar 0x531 and 0x651 from CAN1 to CAN0 + if ((addr == 0x531) || (addr == 0x651)){ + return 0; + } + + //block everything else from radar + return -1; + } + if (bus_num == 2) { // remove GTW_epasControl in forwards if (addr == 0x101) { - return false; + return -1; + } + + // remove Pedal in forwards + if ((addr == 0x520) || (addr == 0x521)) { + return -1; } return 0; // Chassis CAN } - return false; + return -1; } const safety_hooks tesla_hooks = { 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_tesla.py b/panda/tests/pedal/enter_canloader_tesla.py new file mode 100755 index 00000000000000..7f55cd6822537e --- /dev/null +++ b/panda/tests/pedal/enter_canloader_tesla.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python +import sys +import time +import struct +import argparse +import signal +from panda import Panda + +class CanHandle(object): + def __init__(self, p): + self.p = p + + def transact(self, dat): + #print "W:",dat.encode("hex") + self.p.isotp_send(1, dat, 2, recvaddr=2) + + def _handle_timeout(signum, frame): + # will happen on reset + raise Exception("timeout") + + signal.signal(signal.SIGALRM, _handle_timeout) + signal.alarm(1) + try: + ret = self.p.isotp_recv(2, 2, sendaddr=1) + finally: + signal.alarm(0) + + #print "R:",ret.encode("hex") + return ret + + def controlWrite(self, request_type, request, value, index, data, timeout=0): + # ignore data in reply, panda doesn't use it + return self.controlRead(request_type, request, value, index, 0, timeout) + + def controlRead(self, request_type, request, value, index, length, timeout=0): + dat = struct.pack("HHBBHHH", 0, 0, request_type, request, value, index, length) + return self.transact(dat) + + def bulkWrite(self, endpoint, data, timeout=0): + if len(data) > 0x10: + raise ValueError("Data must not be longer than 0x10") + dat = struct.pack("HH", endpoint, len(data))+data + return self.transact(dat) + + def bulkRead(self, endpoint, length, timeout=0): + dat = struct.pack("HH", endpoint, 0) + return self.transact(dat) + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='Flash pedal over can') + parser.add_argument('--recover', action='store_true') + parser.add_argument("fn", type=str, nargs='?', help="flash file") + args = parser.parse_args() + + p = Panda() + p.set_safety_mode(0x1337) + + while 1: + if len(p.can_recv()) == 0: + break + + if args.recover: + p.can_send(0x551, "\xce\xfa\xad\xde\x1e\x0b\xb0\x02", 2) + exit(0) + else: + p.can_send(0x551, "\xce\xfa\xad\xde\x1e\x0b\xb0\x0a", 2) + if args.fn: + time.sleep(0.1) + print "flashing", args.fn + code = open(args.fn).read() + Panda.flash_static(CanHandle(p), code) + + print "can flash done" + + 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/assets/img_car_tesla.png b/selfdrive/assets/img_car_tesla.png new file mode 100644 index 00000000000000..d18b927b7dcc6e Binary files /dev/null and b/selfdrive/assets/img_car_tesla.png differ diff --git a/selfdrive/assets/img_logo_tesla.png b/selfdrive/assets/img_logo_tesla.png new file mode 100644 index 00000000000000..d7796fc37ca897 Binary files /dev/null and b/selfdrive/assets/img_logo_tesla.png differ diff --git a/selfdrive/assets/img_spinner_comma.honda.png b/selfdrive/assets/img_spinner_comma.honda.png new file mode 100644 index 00000000000000..0e4db4dae7cb0c Binary files /dev/null and b/selfdrive/assets/img_spinner_comma.honda.png differ diff --git a/selfdrive/assets/img_spinner_comma.honda2.png b/selfdrive/assets/img_spinner_comma.honda2.png new file mode 100644 index 00000000000000..bc6c475b782129 Binary files /dev/null and b/selfdrive/assets/img_spinner_comma.honda2.png differ diff --git a/selfdrive/assets/img_spinner_comma.png b/selfdrive/assets/img_spinner_comma.png index 16109557f85911..195cbcca5cc11a 100644 Binary files a/selfdrive/assets/img_spinner_comma.png and b/selfdrive/assets/img_spinner_comma.png differ diff --git a/selfdrive/assets/img_spinner_comma.toyota.png b/selfdrive/assets/img_spinner_comma.toyota.png new file mode 100644 index 00000000000000..63d9334491e764 Binary files /dev/null and b/selfdrive/assets/img_spinner_comma.toyota.png differ diff --git a/selfdrive/assets/img_spinner_comma.toyota2.png b/selfdrive/assets/img_spinner_comma.toyota2.png new file mode 100644 index 00000000000000..609b7ab26ba428 Binary files /dev/null and b/selfdrive/assets/img_spinner_comma.toyota2.png differ diff --git a/selfdrive/assets/img_spinner_comma2.png b/selfdrive/assets/img_spinner_comma2.png new file mode 100644 index 00000000000000..f6fd441b004c59 Binary files /dev/null and b/selfdrive/assets/img_spinner_comma2.png differ diff --git a/selfdrive/assets/lock_icon.png b/selfdrive/assets/lock_icon.png new file mode 100644 index 00000000000000..5a6b7bad763a03 Binary files /dev/null and b/selfdrive/assets/lock_icon.png differ diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index aae2b665aec1bb..c3195a6f32af10 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -39,6 +39,7 @@ #define SAFETY_HONDA_BOSCH 4 #define SAFETY_FORD 5 #define SAFETY_CADILLAC 6 +#define SAFETY_TESLA 8 #define SAFETY_HYUNDAI 7 #define SAFETY_TESLA 8 #define SAFETY_TOYOTA_IPAS 0x1335 @@ -117,6 +118,9 @@ void *safety_setter_thread(void *s) { case (int)cereal::CarParams::SafetyModels::CADILLAC: safety_setting = SAFETY_CADILLAC; break; + case (int)cereal::CarParams::SafetyModels::TESLA: + safety_setting = SAFETY_TESLA; + break; case (int)cereal::CarParams::SafetyModels::HYUNDAI: safety_setting = SAFETY_HYUNDAI; break; diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc index 4a82d9a022f36f..effd2f2f86ac88 100644 --- a/selfdrive/can/parser.cc +++ b/selfdrive/can/parser.cc @@ -105,7 +105,12 @@ struct MessageState { tmp -= (tmp >> (sig.b2-1)) ? (1ULL << sig.b2) : 0; //signed } - DEBUG("parse %X %s -> %lld\n", address, sig.name, tmp); + // Testing both little and big endian signals (Tesla messages) + //if ( (address == 0x318) || (address == 0x118)) { + // INFO("parse %X %s -> %f, dat -> %lX\n", address, sig.name, tmp * sig.factor + sig.offset, dat); + //} + + DEBUG("parse %X %s -> %lld\n", address, sig.name, tmp); if (sig.type == SignalType::HONDA_CHECKSUM) { if (honda_checksum(address, dat, size) != tmp) { diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index dbc392cb23e584..d683b17bc24cfd 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -13,6 +13,7 @@ def load_interfaces(x): imp = __import__('selfdrive.car.%s.interface' % interface, fromlist=['CarInterface']).CarInterface except ImportError: imp = None + print "Import Exception on Interface " + interface for car in x[interface]: ret[car] = imp return ret @@ -76,7 +77,9 @@ def fingerprint(logcan, timeout): # bail if no cars left or we've been waiting too long elif len(candidate_cars) == 0 or (timeout and (ts - st_passive) > timeout): - return None, finger + #return None, finger + print "Fingerprinting Failed: Returning Tesla (based on branch)" + return "TESLA MODEL S", finger time.sleep(0.01) diff --git a/selfdrive/car/ford/.gitignore b/selfdrive/car/ford/.gitignore new file mode 100644 index 00000000000000..89fa7bc7daf83e --- /dev/null +++ b/selfdrive/car/ford/.gitignore @@ -0,0 +1,2 @@ +buttons.msg +buttons.cc.msg diff --git a/selfdrive/car/gm/.gitignore b/selfdrive/car/gm/.gitignore new file mode 100644 index 00000000000000..89fa7bc7daf83e --- /dev/null +++ b/selfdrive/car/gm/.gitignore @@ -0,0 +1,2 @@ +buttons.msg +buttons.cc.msg diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 9ec1b9e5d0e12c..d06235488142a6 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -19,6 +19,10 @@ def __init__(self): self.chassis = 2 self.sw_gmlan = 3 +# 384 = "ASCMLKASteeringCmd" +# 715 = "ASCMGasRegenCmd" +CONTROL_MSGS = [384, 715] + class CarInterface(object): def __init__(self, CP, sendcan=None): self.CP = CP diff --git a/selfdrive/car/honda/.gitignore b/selfdrive/car/honda/.gitignore new file mode 100644 index 00000000000000..89fa7bc7daf83e --- /dev/null +++ b/selfdrive/car/honda/.gitignore @@ -0,0 +1,2 @@ +buttons.msg +buttons.cc.msg diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 1cb9830e39f26e..95ad72793f7d3f 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -6,6 +6,7 @@ from selfdrive.car.honda import hondacan from selfdrive.car.honda.values import AH, CruiseButtons, CAR from selfdrive.can.packer import CANPacker +from selfdrive.car.modules.ALCA_module import ALCAController def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint): # hyst params @@ -83,7 +84,7 @@ def __init__(self, dbc_name, enable_camera=True): self.enable_camera = enable_camera self.packer = CANPacker(dbc_name) self.new_radar_config = False - + self.ALCA = ALCAController(self,True,False) # Enabled True and SteerByAngle only False def update(self, sendcan, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ radar_error, hud_v_cruise, hud_show_lanes, hud_show_car, \ @@ -140,10 +141,30 @@ def update(self, sendcan, enabled, CS, frame, actuators, \ else: STEER_MAX = 0x1000 + #update custom UI buttons and alerts + CS.UE.update_custom_ui() + if (frame % 1000 == 0): + CS.cstm_btns.send_button_info() + CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder,CS.cstm_btns.car_name) + + # Get the angle from ALCA. + alca_enabled = False + alca_steer = 0. + alca_angle = 0. + turn_signal_needed = 0 + # Update ALCA status and custom button every 0.1 sec. + if self.ALCA.pid == None: + self.ALCA.set_pid(CS) + if (frame % 10 == 0): + self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0) + # steer torque + alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(enabled, CS, frame, actuators) + + # steer torque is converted back to CAN reference (positive when steering right) apply_gas = clip(actuators.gas, 0., 1.) apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1)) - apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX)) + apply_steer = int(clip(-alca_steer * STEER_MAX, -STEER_MAX, STEER_MAX)) lkas_active = enabled and not CS.steer_not_allowed diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index e6c4ed4de1ef93..7630d62803e220 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -196,7 +196,7 @@ def get_params(candidate, fingerprint): tire_stiffness_factor = 1. # Civic at comma has modified steering FW, so different tuning for the Neo in that car is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e'] - ret.steerKpV, ret.steerKiV = [[0.33], [0.10]] if is_fw_modified else [[0.8], [0.24]] + ret.steerKpV, ret.steerKiV = [[0.33], [0.10]] if is_fw_modified else [[0.6], [0.18]] if is_fw_modified: ret.steerKf = 0.00003 ret.longitudinalKpBP = [0., 5., 35.] @@ -211,7 +211,7 @@ def get_params(candidate, fingerprint): ret.centerToFront = centerToFront_civic ret.steerRatio = 14.63 # 10.93 is spec end-to-end tire_stiffness_factor = 1. - ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] + ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] @@ -256,7 +256,7 @@ def get_params(candidate, fingerprint): ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 15.3 # as spec tire_stiffness_factor = 0.444 # not optimized yet - ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] + ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] @@ -283,7 +283,7 @@ def get_params(candidate, fingerprint): ret.centerToFront = ret.wheelbase * 0.38 ret.steerRatio = 15.0 # as spec tire_stiffness_factor = 0.444 # not optimized yet - ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] + ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] diff --git a/selfdrive/car/modules/ALCA_module.py b/selfdrive/car/modules/ALCA_module.py new file mode 100644 index 00000000000000..ea314df1b22ef3 --- /dev/null +++ b/selfdrive/car/modules/ALCA_module.py @@ -0,0 +1,479 @@ +""" +Copyright 2018 BB Solutions, LLC. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are +met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the + distribution. + + * Neither the name of Google nor the names of its contributors may + be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +HISTORY +------- +v3.6 - moved parameters to carstate.py +v3.5 - changing the start angle to keep turning until we reach MAX_ANGLE_DELTA +v3.4 - read steerRatio from each car parameters file +v3.3 - re-entry logic changed for smoothness +v3.2 - angle adjustment to compesate for road curvature change +v3.1 - new angle logic for a smoother re-entry +v3.0 - better lane dettection logic +v2.0 - detection of lane crossing +v1.0 - fixed angle move + +######################################################## +The following parameters need to be added to carstate.py +and based on each model you want to define +######################################################## + +# max REAL delta angle for correction vs actuator +CL_MAX_ANGLE_DELTA_BP = [10., 44.] +CL_MAX_ANGLE_DELTA = [2., .8] + +# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother +CL_ADJUST_FACTOR_BP = [10., 44.] +CL_ADJUST_FACTOR = [16. , 16.] + + +# reenrey angle when to let go +CL_REENTRY_ANGLE_BP = [10., 44.] +CL_REENTRY_ANGLE = [5. , 5.] + +# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line +CL_LANE_DETECT_BP = [10., 44.] +CL_LANE_DETECT_FACTOR = [1.5, .75] + +CL_LANE_PASS_BP = [10., 44.] +CL_LANE_PASS_TIME = [100., 5.] + +# change lane delta angles and other params +CL_MAXD_BP = [10., 32., 44.] +CL_MAXD_A = [.358, 0.084, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75 + +CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s + +# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed +CL_MAX_A_BP = [10., 44.] +CL_MAX_A = [10., 10.] + +# define limits for angle change every 0.1 s +# we need to force correction above 10 deg but less than 20 +# anything more means we are going to steep or not enough in a turn +CL_MAX_ACTUATOR_DELTA = 2. +CL_MIN_ACTUATOR_DELTA = 0. +CL_CORRECTION_FACTOR = 1. + +#duration after we cross the line until we release is a factor of speed +CL_TIMEA_BP = [10., 32., 44.] +CL_TIMEA_T = [0.7 ,0.30, 0.20] + + +""" + +from common.numpy_fast import interp +from selfdrive.controls.lib.pid import PIController +from common.realtime import sec_since_boot + +#wait time after turn complete before enabling smoother +WAIT_TIME_AFTER_TURN = 2.0 + +class ALCAController(object): + def __init__(self,carcontroller,alcaEnabled,steerByAngle): + #import settings + self.CC = carcontroller # added to start, will see if we need it actually + # variables for lane change + self.angle_offset = 0. #added when one needs to compensate for missalignment + self.alcaEnabled = alcaEnabled + self.laneChange_strStartFactor = 2. + self.laneChange_strStartMultiplier = 1.5 + self.laneChange_steerByAngle = steerByAngle # steer only by angle; do not call PID + 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_after_lane_duration_mult = 1. # multiplier for time after we cross the line before we let OP take over; multiplied with CL_TIMEA_T + self.laneChange_wait = 1 # 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 = 15.75 # steer ratio for lane change starting with the Tesla one + 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 + self.keep_angle = False #local variable to keep certain angle delta vs. actuator + self.pid = None + self.last10delta = [] + self.laneChange_cancelled = False + self.laneChange_cancelled_counter = 0 + self.last_time_enabled = 0 + + + def last10delta_reset(self): + self.last10delta = [] + for i in range (0,10): + self.last10delta.append(0.) + + def last10delta_add(self,angle): + self.last10delta.pop(0) + self.last10delta.append(angle) + n = 0 + a = 0. + for i in self.last10delta: + if i != 0: + n += 1 + a += i + return n,a + + def last10delta_correct(self,c): + for i in self.last10delta: + j = self.last10delta.index(i) + n = self.last10delta.pop(j) + n += c + self.last10delta.insert(j,n) + + + def update_status(self,alcaEnabled): + self.alcaEnabled = alcaEnabled + + def set_pid(self,CS): + self.laneChange_steerr = CS.CP.steerRatio + self.pid = PIController((CS.CP.steerKpBP, CS.CP.steerKpV), + (CS.CP.steerKiBP, CS.CP.steerKiV), + k_f=CS.CP.steerKf, pos_limit=1.0) + + def update_angle(self,enabled,CS,frame,actuators): + alcaMode = CS.cstm_btns.get_button_label2_index("alca") + #parameters that define the speed/aggressiveness of lane change modes + alca_m1 = [1., .9, .8] + alca_m2 = [1., 1., 1.7] + alca_m3 = [1., 0.5, 0.5] + # speed variable parameters + cl_max_angle_delta = interp(CS.v_ego,CS.CL_MAX_ANGLE_DELTA_BP,CS.CL_MAX_ANGLE_DELTA) * alca_m1[alcaMode] + cl_maxd_a = interp(CS.v_ego, CS.CL_MAXD_BP, CS.CL_MAXD_A) * alca_m3[alcaMode] + cl_lane_pass_time = interp(CS.v_ego,CS.CL_LANE_PASS_BP,CS.CL_LANE_PASS_TIME) * alca_m2[alcaMode] + cl_timea_t = interp(CS.v_ego,CS.CL_TIMEA_BP,CS.CL_TIMEA_T) * alca_m2[alcaMode] + cl_lane_detect_factor = interp(CS.v_ego, CS.CL_LANE_DETECT_BP, CS.CL_LANE_DETECT_FACTOR) + cl_max_a = interp(CS.v_ego, CS.CL_MAX_A_BP, CS.CL_MAX_A) + cl_adjust_factor = interp(CS.v_ego, CS.CL_ADJUST_FACTOR_BP, CS.CL_ADJUST_FACTOR) + cl_reentry_angle = interp(CS.v_ego, CS.CL_REENTRY_ANGLE_BP, CS.CL_REENTRY_ANGLE) + cl_min_v = CS.CL_MIN_V + self.laneChange_wait = CS.CL_WAIT_BEFORE_START + + if self.laneChange_cancelled_counter > 0: + self.laneChange_cancelled_counter -= 1 + if self.laneChange_cancelled_counter == 0: + self.laneChange_cancelled = False + + # Basic highway lane change logic + 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 + CS.UE.custom_alert_message(-1,"",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 (CS.cstm_btns.get_button_status("alca") > 0) and 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 + CS.UE.custom_alert_message(3,"Auto Lane Change Unavailable!",500,3) + 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 + CS.UE.custom_alert_message(3,"Auto Lane Change Canceled! (s)",200,5) + self.laneChange_cancelled = True + self.laneChange_cancelled_counter = 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 + CS.UE.custom_alert_message(2,"Auto Lane Change Engaged! (1)",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 * 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) + # reset PID for torque + self.pid.reset() + + 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)): + CS.UE.custom_alert_message(4,"Auto Lane Change Canceled! (u)",200,3) + self.laneChange_cancelled = True + self.laneChange_cancelled_counter = 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) + # now that we crossed the line, we need to get far enough from the line and then engage OP + # 1. we wait 0.05s to ensure we don't have back and forth adjustments + # 2. we check to see if the delta between our steering and the actuator continues to decrease or not + # 3. we check to see if the delta between our steering and the actuator is less than 5 deg + # 4. we disengage after 0.5s and let OP take over + if self.laneChange_enabled ==2: + laneChange_angle = self.laneChange_angled + if self.laneChange_counter ==1: + CS.UE.custom_alert_message(2,"Auto Lane Change Engaged! (5)",800) + self.laneChange_counter += 1 + # check if angle continues to decrease + current_delta = abs(self.laneChange_angle + laneChange_angle + (-actuators.steerAngle)) + previous_delta = abs(self.laneChange_last_sent_angle - self.laneChange_last_actuator_angle) + if (self.laneChange_counter > cl_lane_pass_time): + # continue to half the angle between our angle and actuator + delta_angle = (-actuators.steerAngle - self.laneChange_angle - self.laneChange_angled)/cl_adjust_factor + self.laneChange_angle += delta_angle + # wait 0.05 sec before starting to check if angle increases or if we are within X deg of actuator.angleSteer + if ((current_delta > previous_delta) or (current_delta <= cl_reentry_angle)) and (self.laneChange_counter > cl_lane_pass_time): + self.laneChange_enabled = 7 + self.laneChange_counter = 1 + self.laneChange_direction = 0 + # we crossed the line, so x sec later give control back to OP + laneChange_after_lane_duration = cl_timea_t * self.laneChange_after_lane_duration_mult + if (self.laneChange_counter > laneChange_after_lane_duration * 100): + self.laneChange_enabled = 7 + self.laneChange_counter = 1 + self.laneChange_direction = 0 + # 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) + if self.laneChange_enabled ==3: + if self.laneChange_counter == 1: + CS.UE.custom_alert_message(2,"Auto Lane Change Engaged! (4)",800) + self.laneChange_over_the_line = 0 + self.last10delta_reset() + self.keep_angle = False + 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 * cl_lane_detect_factor): + # sudden change in actuator angle or sign means we are on the other side of the line + self.laneChange_over_the_line = 1 + self.laneChange_enabled = 2 + self.laneChange_counter = 1 + + # didn't change the lane yet, check that we are not eversteering or understeering based on road curvature + + """ + # code for v3.1 designed to turn more if lane moves away + n,a = self.last10delta_add(actuator_delta) + c = 5. + if a == 0: + a = 0.00001 + if (abs(a) < CL_MIN_ACTUATOR_DELTA) and (self.keep_angle == False): + c = (a/abs(a)) * (CL_MIN_ACTUATOR_DELTA - abs(a)) / 10 + self.laneChange_angle = self.laneChange_angle -self.laneChange_direction * CL_CORRECTION_FACTOR * c * 10 + self.last10delta_correct(c) + #print a, c, actuator_delta, self.laneChange_angle + """ + + # code for v3.2 and above + a_delta = self.laneChange_direction * (self.laneChange_angle + laneChange_angle - (-actuators.steerAngle)) + if (self.laneChange_over_the_line == 0) and ((abs(a_delta) > cl_max_angle_delta * self.laneChange_steerr) or (self.keep_angle)): + #steering more than what we wanted, need to adjust + self.keep_angle = True + self.laneChange_angle = -actuators.steerAngle + self.laneChange_direction * cl_max_angle_delta * self.laneChange_steerr - laneChange_angle + if self.laneChange_counter > (self.laneChange_duration) * 100: + self.laneChange_enabled = 1 + self.laneChange_counter = 0 + CS.UE.custom_alert_message(4,"Auto Lane Change Canceled! (t)",200,5) + self.laneChange_cancelled = True + self.laneChange_cancelled_counter = 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 + self.laneChange_angled = self.laneChange_direction * self.laneChange_steerr * cl_maxd_a + # if angle more than max angle allowed cancel; last chance to cancel on road curvature + if self.laneChange_counter == 2: + CS.UE.custom_alert_message(2,"Auto Lane Change Engaged! (3)",100) + if (abs(self.laneChange_angle) > cl_max_a) and (self.laneChange_counter == 1): + CS.UE.custom_alert_message(4,"Auto Lane Change Canceled! (a)",200,5) + self.laneChange_cancelled = True + self.laneChange_cancelled_counter = 200 + self.laneChange_enabled = 1 + self.laneChange_counter = 0 + self.laneChange_direction = 0 + CS.cstm_btns.set_button_status("alca",1) + self.laneChange_counter += 1 + #laneChange_angle = self.laneChange_strStartFactor * self.laneChange_angled * self.laneChange_counter / 50 + #delta_change = abs(self.laneChange_angle+ laneChange_angle + actuators.steerAngle) - self.laneChange_strStartMultiplier * abs(self.laneChange_angled) + laneChange_angle = self.laneChange_strStartFactor * self.laneChange_angled * self.laneChange_counter / 100 + delta_change = abs(self.laneChange_angle + laneChange_angle + actuators.steerAngle) - cl_max_angle_delta * self.laneChange_steerr + if (self.laneChange_counter == 100) or (delta_change >= 0): + if (delta_change < 0): + # didn't achieve desired angle yet, so repeat + self.laneChange_counter = 2 + self.laneChange_angle += laneChange_angle + laneChange_angle = 0. + 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: + CS.UE.custom_alert_message(2,"Auto Lane Change Engaged! (2)",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: + CS.UE.custom_alert_message(2,"Auto Lane Change Complete!",300,4) + 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-self.angle_offset,alca_enabled,turn_signal_needed] + + + + def update(self,enabled,CS,frame,actuators): + if self.alcaEnabled: + # ALCA enabled + cur_time = sec_since_boot() + 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) + if new_ALCA_Enabled: + self.last_time_enabled = sec_since_boot() + output_steer = 0. + if new_ALCA_Enabled and (self.laneChange_enabled < 5 ) and not self.laneChange_steerByAngle: + steers_max = interp(CS.v_ego, CS.CP.steerMaxBP, CS.CP.steerMaxV) + self.pid.pos_limit = steers_max + self.pid.neg_limit = -steers_max + output_steer = self.pid.update(new_angle, CS.angle_steers , check_saturation=(CS.v_ego > 10), override=CS.steer_override, feedforward=new_angle * (CS.v_ego ** 2), speed=CS.v_ego, deadzone=0.0) + else: + output_steer = actuators.steer + if self.laneChange_steerByAngle and (not new_ALCA_Enabled) and (cur_time - self.last_time_enabled > WAIT_TIME_AFTER_TURN): + new_angle = actuators.steer + return [new_angle,output_steer,new_ALCA_Enabled,new_turn_signal] + else: + # ALCA disabled + if self.laneChange_steerByAngle: + # when steerByAngle, actuators.steer has the smoothed version of the angle by Grenby + return [actuators.steer,actuators.steer,False,0] + else: + return [actuators.steerAngle,actuators.steer,False,0] + diff --git a/selfdrive/car/modules/UIBT_module.py b/selfdrive/car/modules/UIBT_module.py new file mode 100644 index 00000000000000..4f9b5a90d03ee1 --- /dev/null +++ b/selfdrive/car/modules/UIBT_module.py @@ -0,0 +1,201 @@ +#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_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,btn_index): + self.btn_name = btn_name + self.btn_label = btn_label + self.btn_label2 = btn_label2 + self.btn_status = btn_status + self.btn_index = btn_index + + +class UIButtons: + def write_buttons_labels_to_file(self): + fo = open(self.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(self.buttons_labels_path, buttons_file_r) + indata = fi.read() + fi.close() + file_matches = False + if len(indata) == btn_msg_len * 6 : + for i in range(0, len(indata), btn_msg_len): + j = int(i/btn_msg_len) + name,label,label2 = struct.unpack(btn_msg_struct, indata[i:i+btn_msg_len]) + if self.btns[j].btn_name == name.rstrip("\0"): + file_matches = True + self.btns[j].btn_label = label.rstrip("\0") + #check if label is actually a valid option + if label2.rstrip("\0") in self.CS.btns_init[j][2]: + self.btns[j].btn_label2 = label2.rstrip("\0") + else: + self.btns[j].btn_label2 = self.CS.btns_init[j][2][0] + return file_matches + else: + #we don't have all the data, ignore + print "labels file is bad" + return False + + + def write_buttons_out_file(self): + if self.hasChanges: + fo = open(self.buttons_status_out_path, buttons_file_rw) + for btn in self.btns: + btn_status = 1 if btn.btn_status > 0 else 0 + fo.write(struct.pack("B",btn_status + 48)) + fo.close() + self.hasChanges = False + + def read_buttons_out_file(self): + fi = open(self.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 send_button_info(self): + if self.isLive: + for i in range(0,6): + self.CS.UE.uiButtonInfoEvent(i,self.btns[i].btn_name, \ + self.btns[i].btn_label,self.btns[i].btn_status,self.btns[i].btn_label2) + + def __init__(self, carstate,car,folder): + self.isLive = False + self.CS = carstate + self.car_folder = folder + self.car_name = car + self.buttons_labels_path = "/data/openpilot/selfdrive/car/"+self.car_folder+"/buttons.msg" + self.buttons_status_out_path = "/data/openpilot/selfdrive/car/"+self.car_folder+"/buttons.cc.msg" + self.btns = [] + self.hasChanges = True + self.last_in_read_time = datetime.min + if os.path.exists(self.buttons_labels_path): + self.init_ui_buttons() + #there is a file, load it + if self.read_buttons_labels_from_file(): + self.read_buttons_out_file() + else: + #no match, so write the new ones + self.hasChanges = True + self.write_buttons_labels_to_file() + self.write_buttons_out_file() + else: + #there is no file, create it + self.init_ui_buttons() + self.hasChanges = True + self.write_buttons_labels_to_file() + self.write_buttons_out_file() + #send events to initiate UI + self.isLive = True + self.send_button_info() + self.CS.UE.uiSetCarEvent(self.car_folder,self.car_name) + + def init_ui_buttons(self): + self.btns = [] + try: + self.CS.init_ui_buttons() + print "Buttons iniatlized with custom CS code" + except AttributeError: + # no init method + print "Buttons iniatlized with just base code" + for i in range(0,len(self.CS.btns_init)): + self.btns.append(UIButton(self.CS.btns_init[i][0],self.CS.btns_init[i][1],1,self.CS.btns_init[i][2][0],i)) + + def get_button(self, btn_name): + for button in self.btns: + if button.btn_name.strip() == btn_name: + return button + return None + + def get_button_status(self, btn_name): + btn = self.get_button(btn_name) + if btn: + return btn.btn_status + else: + return -1 + + def set_button_status(self,btn_name,btn_status): + btn = self.get_button(btn_name) + if btn: + #if we change from enable to disable or the other way, save to file + if btn.btn_status * btn_status == 0 and btn.btn_status != btn_status: + self.hasChanges = True + btn.btn_status = btn_status + self.CS.UE.uiButtonInfoEvent(self.btns.index(btn),btn.btn_name, \ + btn.btn_label,btn.btn_status,btn.btn_label2) + if self.hasChanges: + self.write_buttons_out_file() + + def set_button_status_from_ui(self,id,btn_status): + old_btn_status = self.btns[id].btn_status + old_btn_label2 = self.btns[id].btn_label2 + if old_btn_status * btn_status == 0 and old_btn_status != btn_status: + self.hasChanges = True + self.update_ui_buttons(id,btn_status) + new_btn_status = self.btns[id].btn_status + if new_btn_status * btn_status == 0 and new_btn_status != btn_status: + self.hasChanges = True + if self.hasChanges: + self.CS.UE.uiButtonInfoEvent(id,self.btns[id].btn_name, \ + self.btns[id].btn_label,self.btns[id].btn_status,self.btns[id].btn_label2) + if old_btn_label2 != self.btns[id].btn_label2: + self.write_buttons_labels_to_file() + self.write_buttons_out_file() + + + def get_button_label2(self, btn_name): + btn = self.get_button(btn_name) + if btn: + return btn.btn_label2 + else: + return -1 + + def get_button_label2_index(self, btn_name): + btn = self.get_button(btn_name) + if btn: + b_index = -1 + bpos = self.btns.index(btn) + for i in range(0,len(self.CS.btns_init[bpos][2])): + if btn.btn_label2 == self.CS.btns_init[bpos][2][i]: + b_index = i + return b_index + else: + return -1 + + def update_ui_buttons(self,id,btn_status): + if self.CS.cstm_btns.btns[id].btn_status > 0: + #if we have more than one Label2 then we toggle + if (len(self.CS.btns_init[id][2]) > 1): + status2 = 0 + for i in range(0,len(self.CS.btns_init[id][2])): + if self.CS.cstm_btns.btns[id].btn_label2 == self.CS.btns_init[id][2][i]: + status2 = (i + 1 ) % len(self.CS.btns_init[id][2]) + self.CS.cstm_btns.btns[id].btn_label2 = self.CS.btns_init[id][2][status2] + self.CS.cstm_btns.hasChanges = True + else: + self.CS.cstm_btns.btns[id].btn_status = btn_status * self.CS.cstm_btns.btns[id].btn_status + else: + self.CS.cstm_btns.btns[id].btn_status = btn_status + try: + #try to see if there is custom CS function for updating ui buttons + self.CS.update_ui_buttons(id,btn_status) + except AttributeError: + # no update method + print "Buttons updated with just base code" diff --git a/selfdrive/car/modules/UIEV_module.py b/selfdrive/car/modules/UIEV_module.py new file mode 100644 index 00000000000000..24975f5936ab6a --- /dev/null +++ b/selfdrive/car/modules/UIEV_module.py @@ -0,0 +1,88 @@ +from cereal import ui +from common import realtime +import selfdrive.messaging as messaging +from selfdrive.services import service_list +import zmq + +class UIEvents(object): + def __init__(self,carstate): + self.CS = carstate + context = zmq.Context() + self.buttons_poller = zmq.Poller() + self.uiCustomAlert = messaging.pub_sock(context, service_list['uiCustomAlert'].port) + self.uiButtonInfo = messaging.pub_sock(context, service_list['uiButtonInfo'].port) + self.uiSetCar = messaging.pub_sock(context, service_list['uiSetCar'].port) + self.uiPlaySound = messaging.pub_sock(context, service_list['uiPlaySound'].port) + self.uiButtonStatus = messaging.sub_sock(context, service_list['uiButtonStatus'].port, conflate=True, poller=self.buttons_poller) + self.prev_cstm_message = "" + self.prev_cstm_status = -1 + + def uiCustomAlertEvent(self,status,message): + dat = ui.UICustomAlert.new_message() + dat.caStatus = status + dat.caText = message+'\0' + self.uiCustomAlert.send(dat.to_bytes()) + + def uiButtonInfoEvent(self,btnid,name,label,status,label2): + dat = ui.UIButtonInfo.new_message() + dat.btnId = btnid + dat.btnName = name #+ '\0' + dat.btnLabel = label #+ '\0' + dat.btnStatus = status + dat.btnLabel2 = label2 #+ '\0' + self.uiButtonInfo.send(dat.to_bytes()) + + def uiSetCarEvent(self,car_folder,car_name): + dat = ui.UISetCar.new_message() + dat.icCarFolder = car_folder + dat.icCarName = car_name + self.uiSetCar.send(dat.to_bytes()) + + def uiPlaySoundEvent(self,sound): + if self.CS.cstm_btns.get_button_status("sound") > 0: + dat = ui.UIPlaySound.new_message() + dat.sndSound = sound + self.uiPlaySound.send(dat.to_bytes()) + + # for status we will use one of these values + # NO_STATUS_ALTERATION -1 + # STATUS_STOPPED 0 + # STATUS_DISENGAGED 1 + # STATUS_ENGAGED 2 + # STATUS_WARNING 3 + # STATUS_ALERT 4 + # STATUS_MAX 5 + + #for sound we will use one of these values + # NO_SOUND -1 + # disable.wav 1 + # enable.wav 2 + # info.wav 3 + # attention.wav 4 + # error.wav 5 + + def custom_alert_message(self,status,message,duration,sound=-1): + if (status > -1) and (self.prev_cstm_status > status) and \ + (self.CS.custom_alert_counter > 55): + #dont change lessage to a lower importance one if we still have more than half second of display time + return + if (sound > -1) and ((self.prev_cstm_message != message) or (self.prev_cstm_status != status)): + self.uiPlaySoundEvent(sound) + self.uiCustomAlertEvent(status,message) + self.CS.custom_alert_counter = duration + self.prev_cstm_message = message + self.prev_cstm_status = status + + def update_custom_ui(self): + btn_message = None + for socket, event in self.buttons_poller.poll(0): + if socket is self.uiButtonStatus: + btn_message = ui.UIButtonStatus.from_bytes(socket.recv()) + if btn_message is not None: + btn_id = btn_message.btnId + self.CS.cstm_btns.set_button_status_from_ui(btn_id,btn_message.btnStatus) + if (self.CS.custom_alert_counter > 0): + self.CS.custom_alert_counter -= 1 + if (self.CS.custom_alert_counter ==0): + self.custom_alert_message(-1,"",0) + self.CS.custom_alert_counter = -1 diff --git a/selfdrive/car/modules/__init__.py b/selfdrive/car/modules/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/car/modules/ch_visiond.sh b/selfdrive/car/modules/ch_visiond.sh new file mode 100755 index 00000000000000..6eca096cd12798 --- /dev/null +++ b/selfdrive/car/modules/ch_visiond.sh @@ -0,0 +1,47 @@ +#!/usr/bin/bash +cd /data/openpilot/selfdrive/visiond +if [ "$1" = "wiggly" ]; then + if [ "$(readlink ./visiond)" != "./visiond-wiggly" ]; then + ln -sf ./visiond-wiggly ./visiond + sleep 2 + echo "Rebooting" + reboot + fi +else + if [ "$1" = "normal" ]; then + if [ "$(readlink ./visiond)" != "./visiond-normal" ]; then + ln -sf ./visiond-normal ./visiond + sleep 2 + echo "Rebooting" + reboot + fi + fi +fi + + + +#tmux has-session -t comma +## if [ $? == 0 ] +#then +# tmux kill-session -t comma +# sleep 1 +#fi +#tmux has-session -t comma +#while [ $? == 0 ] +#do +# sleep 1 +# tmux has-session -t comma +#done +#tmux new-session -d -s comma +#sleep 1 +#tmux has-session -t comma +#while [ $? != 0 ] +#do +# tmux new-session -d -s comma +# sleep 2 +# tmux has-session -t comma +#done + +#tmux send-keys -t comma '/data/data/com.termux/files/continue.sh' C-m + + diff --git a/selfdrive/car/modules/snd/attention.wav b/selfdrive/car/modules/snd/attention.wav new file mode 100644 index 00000000000000..2e9981e4d03020 Binary files /dev/null and b/selfdrive/car/modules/snd/attention.wav differ diff --git a/selfdrive/car/modules/snd/disable.wav b/selfdrive/car/modules/snd/disable.wav new file mode 100644 index 00000000000000..ecc82aa8e2a841 Binary files /dev/null and b/selfdrive/car/modules/snd/disable.wav differ diff --git a/selfdrive/car/modules/snd/enable.wav b/selfdrive/car/modules/snd/enable.wav new file mode 100644 index 00000000000000..91ada8773fe38e Binary files /dev/null and b/selfdrive/car/modules/snd/enable.wav differ diff --git a/selfdrive/car/modules/snd/error.wav b/selfdrive/car/modules/snd/error.wav new file mode 100644 index 00000000000000..81154c225ddd36 Binary files /dev/null and b/selfdrive/car/modules/snd/error.wav differ diff --git a/selfdrive/car/modules/snd/info.wav b/selfdrive/car/modules/snd/info.wav new file mode 100644 index 00000000000000..97f71bf8738c53 Binary files /dev/null and b/selfdrive/car/modules/snd/info.wav differ diff --git a/selfdrive/car/modules/snd/libnative-lib.so b/selfdrive/car/modules/snd/libnative-lib.so new file mode 100644 index 00000000000000..e184efb433f10e Binary files /dev/null and b/selfdrive/car/modules/snd/libnative-lib.so differ diff --git a/selfdrive/car/modules/snd/mediaplayer b/selfdrive/car/modules/snd/mediaplayer new file mode 100755 index 00000000000000..bf79439f8040c2 Binary files /dev/null and b/selfdrive/car/modules/snd/mediaplayer differ diff --git a/selfdrive/car/modules/snd/playsound.py b/selfdrive/car/modules/snd/playsound.py new file mode 100644 index 00000000000000..2b3981901b5d76 --- /dev/null +++ b/selfdrive/car/modules/snd/playsound.py @@ -0,0 +1,27 @@ +import os +import subprocess +import sys + +"call the snd library""" +base_fld_code = "/data/openpilot/selfdrive/car/modules/snd/" +base_fld_wav = "/data/openpilot/selfdrive/car/modules/snd/" +snd_command = base_fld_wav +snd_beep = int(sys.argv[1]) +gen_snd = True +if (snd_beep == 0): #no beep + gen_snd = False +elif (snd_beep == 2): + snd_command += "enable.wav" +elif (snd_beep == 1): + snd_command += "disable.wav" +elif (snd_beep == 4): + snd_command +="attention.wav" +elif (snd_beep == 3): + snd_command += "info.wav" +else: + snd_command += "error.wav" +if (gen_snd): + env = dict(os.environ) + env['LD_LIBRARY_PATH'] = base_fld_code + "." + args = [base_fld_code + "mediaplayer", snd_command] + subprocess.Popen(args, shell = False, stdin=None, stdout=None, stderr=None, env = env, close_fds=True) diff --git a/selfdrive/car/tesla/.gitignore b/selfdrive/car/tesla/.gitignore new file mode 100644 index 00000000000000..89fa7bc7daf83e --- /dev/null +++ b/selfdrive/car/tesla/.gitignore @@ -0,0 +1,2 @@ +buttons.msg +buttons.cc.msg diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py new file mode 100644 index 00000000000000..2746be9cd68728 --- /dev/null +++ b/selfdrive/car/tesla/ACC_module.py @@ -0,0 +1,440 @@ +from selfdrive.services import service_list +from selfdrive.car.tesla.values import ACCMode, ACCState, AH, CruiseButtons, CruiseState, CAR +from selfdrive.config import Conversions as CV +import selfdrive.messaging as messaging +import os +import collections +import subprocess +import sys +import time +import zmq + + +class ACCState(object): + # Possible states of the ACC system, following the DI_cruiseState naming + # scheme. + OFF = 0 # Disabled by UI. + STANDBY = 1 # Ready to be enaged. + ENABLED = 2 # Engaged. + NOT_READY = 9 # Not ready to be engaged due to the state of the car. + +class _Mode(object): + def __init__(self, label, autoresume, state): + self.label = label + self.autoresume = autoresume + self.state = state + self.next = None + +class ACCMode(object): + # Possible ACC modes, controlling how ACC behaves. + # This is separate from ACC state. For example, you could + # have ACC in "Autoresume" mode in "Standby" state. + FOLLOW = _Mode(label="follow", autoresume=False, state=ACCState.STANDBY) + AUTO = _Mode(label="auto", autoresume=True, state=ACCState.STANDBY) + + BUTTON_NAME = 'acc' + BUTTON_ABREVIATION = 'ACC' + + # Toggle order: OFF -> ON -> AUTO -> OFF + _all_modes = [FOLLOW, AUTO] + for index, mode in enumerate(_all_modes): + mode.next = _all_modes[(index + 1) % len(_all_modes)] + + # Map labels to modes for fast lookup by label. + _label_to_mode = {mode.label: mode for mode in _all_modes} + @ classmethod + def from_label(cls, label): + return cls._label_to_mode.get(label, cls.FOLLOW) + + @ classmethod + def labels(cls): + return [mode.label for mode in cls._all_modes] + +def _current_time_millis(): + return int(round(time.time() * 1000)) + + +class ACCController(object): + + # Tesla cruise only functions above 17 MPH + MIN_CRUISE_SPEED_MS = 17.1 * CV.MPH_TO_MS + + def __init__(self): + self.human_cruise_action_time = 0 + self.automated_cruise_action_time = 0 + context = zmq.Context() + self.poller = zmq.Poller() + self.live20 = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=self.poller) + self.last_update_time = 0 + self.enable_adaptive_cruise = False + self.prev_enable_adaptive_cruise = False + # Whether to re-engage automatically after being paused due to low speed or + # user-initated deceleration. + self.autoresume = False + self.last_cruise_stalk_pull_time = 0 + self.prev_cruise_buttons = CruiseButtons.IDLE + self.prev_pcm_acc_status = 0 + self.acc_speed_kph = 0. + self.user_has_braked = False + self.has_gone_below_min_speed = False + self.fast_decel_time = 0 + self.lead_last_seen_time_ms = 0 + # BB speed for testing + self.new_speed = 0 + + # Updates the internal state of this controller based on user input, + # specifically the steering wheel mounted cruise control stalk, and OpenPilot + # UI buttons. + 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. + self.prev_enable_adaptive_cruise = self.enable_adaptive_cruise + acc_string = CS.cstm_btns.get_button_label2(ACCMode.BUTTON_NAME) + acc_mode = ACCMode.from_label(acc_string) + CS.cstm_btns.get_button(ACCMode.BUTTON_NAME).btn_label2 = acc_mode.label + self.autoresume = acc_mode.autoresume + curr_time_ms = _current_time_millis() + # Handle pressing the enable button. + 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 + self.last_cruise_stalk_pull_time = curr_time_ms + ready = (CS.cstm_btns.get_button_status(ACCMode.BUTTON_NAME) > ACCState.OFF + and enabled + and CruiseState.is_enabled_or_standby(CS.pcm_acc_status) + and CS.v_ego > self.MIN_CRUISE_SPEED_MS) + if ready and double_pull: + # A double pull enables ACC. updating the max ACC speed if necessary. + self.enable_adaptive_cruise = True + # Increase ACC speed to match current, if applicable. + self.acc_speed_kph = max(CS.v_ego_raw * CV.MS_TO_KPH, self.acc_speed_kph) + self.user_has_braked = False + self.has_gone_below_min_speed = False + else: + # A single pull disables ACC (falling back to just steering). + self.enable_adaptive_cruise = False + # Handle pressing the cancel button. + elif CS.cruise_buttons == CruiseButtons.CANCEL: + self.enable_adaptive_cruise = False + self.acc_speed_kph = 0. + self.last_cruise_stalk_pull_time = 0 + # Handle pressing up and down buttons. + elif (self.enable_adaptive_cruise and + CS.cruise_buttons != self.prev_cruise_buttons): + self._update_max_acc_speed(CS) + + if CS.brake_pressed: + self.user_has_braked = True + if not self.autoresume: + self.enable_adaptive_cruise = False + + if CS.v_ego < self.MIN_CRUISE_SPEED_MS: + self.has_gone_below_min_speed = True + + # If autoresume is not enabled, manually steering or slowing disables ACC. + if not self.autoresume: + if not enabled or self.user_has_braked or self.has_gone_below_min_speed: + self.enable_adaptive_cruise = False + + # Notify if ACC was toggled + if self.prev_enable_adaptive_cruise and not self.enable_adaptive_cruise: + CS.UE.custom_alert_message(3, "ACC Disabled", 150, 4) + CS.cstm_btns.set_button_status(ACCMode.BUTTON_NAME, ACCState.STANDBY) + elif self.enable_adaptive_cruise: + CS.cstm_btns.set_button_status(ACCMode.BUTTON_NAME, ACCState.ENABLED) + if not self.prev_enable_adaptive_cruise: + CS.UE.custom_alert_message(2, "ACC Enabled", 150) + + # Update the UI to show whether the current car state allows ACC. + if CS.cstm_btns.get_button_status(ACCMode.BUTTON_NAME) in [ACCState.STANDBY, ACCState.NOT_READY]: + if (enabled + and CruiseState.is_enabled_or_standby(CS.pcm_acc_status) + and CS.v_ego > self.MIN_CRUISE_SPEED_MS): + CS.cstm_btns.set_button_status(ACCMode.BUTTON_NAME, ACCState.STANDBY) + else: + CS.cstm_btns.set_button_status(ACCMode.BUTTON_NAME, ACCState.NOT_READY) + + # Update prev state after all other actions. + self.prev_cruise_buttons = CS.cruise_buttons + self.prev_pcm_acc_status = CS.pcm_acc_status + + def _update_max_acc_speed(self, CS): + # Adjust the max ACC speed based on user cruise stalk actions. + half_press_kph, full_press_kph = self._get_cc_units_kph(CS.imperial_speed_units) + speed_change_map = { + CruiseButtons.RES_ACCEL: half_press_kph, + CruiseButtons.RES_ACCEL_2ND: full_press_kph, + CruiseButtons.DECEL_SET: -1 * half_press_kph, + CruiseButtons.DECEL_2ND: -1 * full_press_kph + } + self.acc_speed_kph += speed_change_map.get(CS.cruise_buttons, 0) + + # Clip ACC speed between 0 and 170 KPH. + self.acc_speed_kph = min(self.acc_speed_kph, 170) + self.acc_speed_kph = max(self.acc_speed_kph, 0) + + + # Decide which cruise control buttons to simluate to get the car to the + # desired speed. + def update_acc(self, enabled, CS, frame, actuators, pcm_speed): + # Adaptive cruise control + current_time_ms = _current_time_millis() + if CruiseButtons.should_be_throttled(CS.cruise_buttons): + self.human_cruise_action_time = current_time_ms + button_to_press = None + + # If ACC is disabled, disengage traditional cruise control. + if (self.prev_enable_adaptive_cruise and not self.enable_adaptive_cruise + and CS.pcm_acc_status == CruiseState.ENABLED): + button_to_press = CruiseButtons.CANCEL + + if self.enable_adaptive_cruise and enabled: + if CS.cstm_btns.get_button_label2(ACCMode.BUTTON_NAME) in ["OP", "AutoOP"]: + button_to_press = self._calc_button(CS, pcm_speed) + self.new_speed = pcm_speed * CV.MS_TO_KPH + else: + # Alternative speed decision logic that uses the lead car's distance + # and speed more directly. + # Bring in the lead car distance from the Live20 feed + lead_1 = None + if enabled: + for socket, _ in self.poller.poll(0): + if socket is self.live20: + lead_1 = messaging.recv_one(socket).live20.leadOne + if lead_1.dRel: + self.lead_last_seen_time_ms = current_time_ms + button_to_press = self._calc_follow_button(CS, lead_1) + if button_to_press: + self.automated_cruise_action_time = current_time_ms + # If trying to slow below the min cruise speed, just cancel cruise. + # This prevents a SCCM crash which is triggered by repeatedly pressing + # stalk-down when already at min cruise speed. + if (CruiseButtons.is_decel(button_to_press) + and CS.v_cruise_actual - 1 < self.MIN_CRUISE_SPEED_MS * CV.MS_TO_KPH): + button_to_press = CruiseButtons.CANCEL + if button_to_press == CruiseButtons.CANCEL: + self.fast_decel_time = current_time_ms + # Debug logging (disable in production to reduce latency of commands) + #print "***ACC command: %s***" % button_to_press + return button_to_press + + # function to calculate the cruise button based on a safe follow distance + def _calc_follow_button(self, CS, lead_car): + if lead_car is None: + return None + # Desired gap (in seconds) between cars. + follow_time_s = 2.0 + # v_ego is in m/s, so safe_dist_m is in meters. + safe_dist_m = CS.v_ego * follow_time_s + current_time_ms = _current_time_millis() + # Make sure we were able to populate lead_1. + # dRel is in meters. + lead_dist_m = lead_car.dRel + lead_speed_kph = (lead_car.vRel + CS.v_ego) * CV.MS_TO_KPH + # Relative velocity between the lead car and our set cruise speed. + future_vrel_kph = lead_speed_kph - CS.v_cruise_actual + # How much we can accelerate without exceeding the max allowed speed. + available_speed_kph = self.acc_speed_kph - CS.v_cruise_actual + half_press_kph, full_press_kph = self._get_cc_units_kph(CS.imperial_speed_units) + # button to issue + button = None + # debug msg + msg = None + + # Automatically engage traditional cruise if ACC is active. + if self._should_autoengage_cc(CS, lead_car=lead_car) and self._no_action_for(milliseconds=100): + button = CruiseButtons.RES_ACCEL + # If traditional cruise is engaged, then control it. + elif CS.pcm_acc_status == CruiseState.ENABLED: + + # Disengage cruise control if a slow object is seen ahead. This triggers + # full regen braking, which is stronger than the braking that happens if + # you just reduce cruise speed. + if self._fast_decel_required(CS, lead_car) and self._no_human_action_for(milliseconds=500): + msg = "Off (Slow traffic)" + button = CruiseButtons.CANCEL + self.new_speed = 1 + + # if cruise is set to faster than the max speed, slow down + elif CS.v_cruise_actual > self.acc_speed_kph and self._no_action_for(milliseconds=300): + msg = "Slow to max" + button = CruiseButtons.DECEL_SET + self.new_speed = self.acc_speed_kph + + elif (# if we have a populated lead_distance + lead_dist_m > 0 + and self._no_action_for(milliseconds=300) + # and we're moving + and CS.v_cruise_actual > full_press_kph): + ### Slowing down ### + # Reduce speed significantly if lead_dist < safe dist + # and if the lead car isn't already pulling away. + if lead_dist_m < safe_dist_m * .5 and future_vrel_kph < 2: + msg = "-5 (Significantly too close)" + button = CruiseButtons.DECEL_2ND + self.new_speed = CS.v_ego * CV.MS_TO_KPH - full_press_kph + # Don't rush up to lead car + elif future_vrel_kph < -15: + msg = "-5 (approaching too fast)" + button = CruiseButtons.DECEL_2ND + self.new_speed = CS.v_ego * CV.MS_TO_KPH - full_press_kph + elif future_vrel_kph < -8: + msg = "-1 (approaching too fast)" + button = CruiseButtons.DECEL_SET + self.new_speed = CS.v_ego * CV.MS_TO_KPH - half_press_kph + elif lead_dist_m < safe_dist_m and future_vrel_kph <= 0: + msg = "-1 (Too close)" + button = CruiseButtons.DECEL_SET + self.new_speed = CS.v_ego * CV.MS_TO_KPH - half_press_kph + # Make slow adjustments if close to the safe distance. + # only adjust every 1 secs + elif (lead_dist_m < safe_dist_m * 1.3 + and future_vrel_kph < -1 * half_press_kph + and self._no_action_for(milliseconds=1000)): + msg = "-1 (Near safe distance)" + button = CruiseButtons.DECEL_SET + self.new_speed = CS.v_ego * CV.MS_TO_KPH - half_press_kph + + ### Speed up ### + elif (available_speed_kph > half_press_kph + and lead_dist_m > safe_dist_m + and self._no_human_action_for(milliseconds=1000)): + lead_is_far = lead_dist_m > safe_dist_m * 1.75 + closing = future_vrel_kph < -2 + lead_is_pulling_away = future_vrel_kph > 4 + if lead_is_far and not closing or lead_is_pulling_away: + msg = "+1 (Beyond safe distance and speed)" + button = CruiseButtons.RES_ACCEL + self.new_speed = CS.v_ego * CV.MS_TO_KPH + half_press_kph + + # If lead_dist is reported as 0, no one is detected in front of you so you + # can speed up. Only accel on straight-aways; vision radar often + # loses lead car in a turn. + elif (lead_dist_m == 0 + and CS.angle_steers < 2.0 + and half_press_kph < available_speed_kph + and self._no_action_for(milliseconds=500) + and self._no_human_action_for(milliseconds=1000) + and current_time_ms > self.lead_last_seen_time_ms + 4000): + msg = "+1 (road clear)" + button = CruiseButtons.RES_ACCEL + self.new_speed = CS.v_ego * CV.MS_TO_KPH + half_press_kph + + if (current_time_ms > self.last_update_time + 1000): + ratio = 0 + if safe_dist_m > 0: + ratio = (lead_dist_m / safe_dist_m) * 100 + print "Ratio: {0:.1f}% lead: {1:.1f}m avail: {2:.1f}kph vRel: {3:.1f}kph Angle: {4:.1f}deg".format( + ratio, lead_dist_m, available_speed_kph, lead_car.vRel * CV.MS_TO_KPH, CS.angle_steers) + self.last_update_time = current_time_ms + if msg != None: + print "ACC: " + msg + return button + + def _should_autoengage_cc(self, CS, lead_car=None): + # Automatically (re)engage cruise control so long as + # 1) The carstate allows cruise control + # 2) There is no imminent threat of collision + # 3) The user did not cancel ACC by pressing the brake + cruise_ready = (self.enable_adaptive_cruise + and CS.pcm_acc_status == CruiseState.STANDBY + and CS.v_ego >= self.MIN_CRUISE_SPEED_MS + and _current_time_millis() > self.fast_decel_time + 2000) + + slow_lead = lead_car and lead_car.dRel > 0 and lead_car.vRel < 0 or self._fast_decel_required(CS, lead_car) + + # "Autoresume" mode allows cruise to engage even after brake events, but + # shouldn't trigger DURING braking. + autoresume_ready = self.autoresume and CS.a_ego >= 0.1 + + braked = self.user_has_braked or self.has_gone_below_min_speed + + return cruise_ready and not slow_lead and (autoresume_ready or not braked) + + def _fast_decel_required(self, CS, lead_car): + """ Identifies situations which call for rapid deceleration. """ + if not lead_car or not lead_car.dRel: + return False + + collision_imminent = self._seconds_to_collision(CS, lead_car) < 4 + + lead_absolute_speed_ms = lead_car.vRel + CS.v_ego + lead_too_slow = lead_absolute_speed_ms < self.MIN_CRUISE_SPEED_MS + + return collision_imminent or lead_too_slow + + def _seconds_to_collision(self, CS, lead_car): + if not lead_car or not lead_car.dRel: + return sys.maxint + elif lead_car.vRel >= 0: + return sys.maxint + return abs(float(lead_car.dRel) / lead_car.vRel) + + def _get_cc_units_kph(self, is_imperial_units): + # Cruise control buttons behave differently depending on whether the car + # is configured for metric or imperial units. + if is_imperial_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 + return half_press_kph, full_press_kph + + # Adjust speed based off OP's longitudinal model. As of OpenPilot 0.5.3, this + # is inoperable because the planner crashes when given only visual radar + # inputs. (Perhaps this can be used in the future with a radar install, or if + # OpenPilot planner changes.) + def _calc_button(self, CS, desired_speed_ms): + button_to_press = None + # Automatically engange traditional cruise if appropriate. + if self._should_autoengage_cc(CS) and desired_speed_ms >= CS.v_ego: + button_to_press = CruiseButtons.RES_ACCEL + # If traditional cruise is engaged, then control it. + elif (CS.pcm_acc_status == CruiseState.ENABLED + # But don't make adjustments if a human has manually done so in + # the last 3 seconds. Human intention should not be overridden. + and self._no_human_action_for(milliseconds=3000) + and self._no_automated_action_for(milliseconds=500)): + # The difference between OP's target speed and the current cruise + # control speed, in KPH. + speed_offset_kph = (desired_speed_ms * CV.MS_TO_KPH - CS.v_cruise_actual) + + half_press_kph, full_press_kph = self._get_cc_units_kph(CS.imperial_speed_units) + + # Reduce cruise speed significantly if necessary. Multiply by a % to + # make the car slightly more eager to slow down vs speed up. + if desired_speed_ms < self.MIN_CRUISE_SPEED_MS: + button_to_press = CruiseButtons.CANCEL + if speed_offset_kph < -2 * full_press_kph and CS.v_cruise_actual > 0: + button_to_press = CruiseButtons.CANCEL + elif speed_offset_kph < -0.6 * full_press_kph and CS.v_cruise_actual > 0: + # Send cruise stalk dn_2nd. + button_to_press = CruiseButtons.DECEL_2ND + # Reduce speed slightly if necessary. + elif speed_offset_kph < -0.9 * half_press_kph and CS.v_cruise_actual > 0: + # Send cruise stalk dn_1st. + button_to_press = CruiseButtons.DECEL_SET + # Increase cruise speed if possible. + elif CS.v_ego > self.MIN_CRUISE_SPEED_MS: + # How much we can accelerate without exceeding max allowed speed. + available_speed_kph = self.acc_speed_kph - CS.v_cruise_actual + if speed_offset_kph >= full_press_kph and full_press_kph < available_speed_kph: + # Send cruise stalk up_2nd. + button_to_press = CruiseButtons.RES_ACCEL_2ND + elif speed_offset_kph >= half_press_kph and half_press_kph < available_speed_kph: + # Send cruise stalk up_1st. + button_to_press = CruiseButtons.RES_ACCEL + return button_to_press + + def _no_human_action_for(self, milliseconds): + return _current_time_millis() > self.human_cruise_action_time + milliseconds + + def _no_automated_action_for(self, milliseconds): + return _current_time_millis() > self.automated_cruise_action_time + milliseconds + + def _no_action_for(self, milliseconds): + return self._no_human_action_for(milliseconds) and self._no_automated_action_for(milliseconds) \ No newline at end of file diff --git a/selfdrive/car/tesla/HSO_module.py b/selfdrive/car/tesla/HSO_module.py new file mode 100644 index 00000000000000..cbfaf99d7d98f3 --- /dev/null +++ b/selfdrive/car/tesla/HSO_module.py @@ -0,0 +1,42 @@ +#human steer override module +from selfdrive.config import Conversions as CV +import selfdrive.messaging as messaging +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.enableHSO 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.enableHSO: + if (frame - self.frame_humanSteered < 50): + human_control = True + #CS.cstm_btns.set_button_status("steer",3) + CS.UE.custom_alert_message(3,"Manual Steering Enabled",51,4) + #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..14edc2f5abfb78 --- /dev/null +++ b/selfdrive/car/tesla/PCC_module.py @@ -0,0 +1,644 @@ +from selfdrive.car.tesla import teslacan +from selfdrive.car.tesla.longcontrol_tesla import LongControl, LongCtrlState, STARTING_TARGET_SPEED +from selfdrive.car.tesla import teslacan +from common.numpy_fast import clip, interp +from selfdrive.services import service_list +from selfdrive.car.tesla.values import AH,CruiseState, CruiseButtons, CAR +from selfdrive.boardd.boardd import can_list_to_can_capnp +from selfdrive.config import Conversions as CV +from selfdrive.controls.lib.speed_smoother import speed_smoother +from common.realtime import sec_since_boot +import selfdrive.messaging as messaging +import os +import subprocess +import time +import zmq +import math +import numpy as np +from collections import OrderedDict + + +_DT = 0.05 # 20Hz in our case, since we don't want to process more than once the same live20 message +_DT_MPC = 0.05 # 20Hz + +# TODO: these should end up in values.py at some point, probably variable by trim +# Accel limits +MAX_PEDAL_VALUE = 112. +PEDAL_HYST_GAP = 1.0 # don't change pedal command for small oscilalitons within this value +# Cap the pedal to go from 0 to max in 4 seconds +PEDAL_MAX_UP = MAX_PEDAL_VALUE * _DT / 4 +# Cap the pedal to go from max to 0 in 0.4 seconds +PEDAL_MAX_DOWN = MAX_PEDAL_VALUE * _DT / 0.4 + +# min safe distance in meters. Roughly 2 car lengths. +MIN_SAFE_DIST_M = 10. + +#BBTODO: move the vehicle variables; maybe make them speed variable +TORQUE_LEVEL_ACC = 0. +TORQUE_LEVEL_DECEL = -30. +FOLLOW_TIME_S = 1.5 # time in seconds to follow car in front +MIN_PCC_V_KPH = 0. # +MAX_PCC_V_KPH = 170. + +MIN_CAN_SPEED = 0.3 #TODO: parametrize this in car interface + +# Pull the cruise stalk twice in this many ms for a 'double pull' +STALK_DOUBLE_PULL_MS = 750 + +# Map of speed to max allowed decel. +# Make sure these accelerations are smaller than mpc limits. +_A_CRUISE_MIN = OrderedDict([ + # (speed in m/s, allowed deceleration) + (0.0, 2), + (40., 2)]) + +# Map of speed to max allowed acceleration. +# Need higher accel at very low speed for stop and go. +# make sure these accelerations are smaller than mpc limits. +_A_CRUISE_MAX = OrderedDict([ + # (speed in m/s, allowed acceleration) + (0.0, 0.50), + (5.0, 0.30), + (10., 0.22), + (20., 0.17), + (40., 0.14)]) + +# Lookup table for turns +_A_TOTAL_MAX = OrderedDict([ + (0.0, 2.5), + (20., 2.5), + (40., 2.5)]) + +class Mode(object): + label = None + +class OpMode(Mode): + label = 'OP' + +class FollowMode(Mode): + label = 'FOLLOW' + +class PCCModes(object): + _all_modes = [OpMode(), FollowMode()] + _mode_map = {mode.label : mode for mode in _all_modes} + BUTTON_NAME = 'pedal' + BUTTON_ABREVIATION = 'PDL' + + @classmethod + def from_label(cls, label): + return cls._mode_map.get(label, OpMode()) + + @classmethod + def from_buttons(cls, cstm_btns): + return cls.from_label(cstm_btns.get_button_label2(cls.BUTTON_NAME)) + + @classmethod + def is_selected(cls, mode, cstm_butns): + """Tell if the UI buttons are set to the given mode""" + return type(mode) == type(cls.from_buttons(cstm_butns)) + + @classmethod + def labels(cls): + return [mode.label for mode in cls._all_modes] + + +def tesla_compute_gb(accel, speed): + return float(accel) / 3. + +def calc_cruise_accel_limits(CS, lead): + a_cruise_min = _interp_map(CS.v_ego, _A_CRUISE_MIN) + a_cruise_max = _interp_map(CS.v_ego, _A_CRUISE_MAX) + + a_while_turning_max = max_accel_in_turns(CS.v_ego, CS.angle_steers, CS.CP) + a_cruise_max = min(a_cruise_max, a_while_turning_max) + # Reduce accel if lead car is close + a_cruise_max *= _accel_limit_multiplier(CS.v_ego, lead) + # Reduce decel if lead car is distant + a_cruise_min *= _decel_limit_multiplier(CS.v_ego, lead) + + return float(a_cruise_min), float(a_cruise_max) + + +def max_accel_in_turns(v_ego, angle_steers, CP): + """ + This function returns a limited long acceleration allowed, depending on the existing lateral acceleration + this should avoid accelerating when losing the target in turns + """ + + a_total_max = _interp_map(v_ego, _A_TOTAL_MAX) + a_y = v_ego**2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase) + a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.)) + return a_x_allowed + + +class PCCState(object): + # Possible state of the ACC system, following the DI_cruiseState naming + # scheme. + OFF = 0 # Disabled by UI. + STANDBY = 1 # Ready to be enaged. + ENABLED = 2 # Engaged. + NOT_READY = 9 # Not ready to be engaged due to the state of the car. + + +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.stalk_pull_time_ms = 0 + self.prev_stalk_pull_time_ms = -1000 + self.prev_pcm_acc_status = 0 + self.prev_cruise_buttons = CruiseButtons.IDLE + self.pedal_speed_kph = 0. + self.pedal_idx = 0 + self.pedal_steady = 0. + self.prev_tesla_accel = 0. + self.prev_tesla_pedal = 0. + self.pedal_interceptor_state = 0 + self.torqueLevel_last = 0. + self.prev_v_ego = 0. + self.PedalForZeroTorque = 18. #starting number, works on my S85 + self.lastTorqueForPedalForZeroTorque = TORQUE_LEVEL_DECEL + self.v_pid = 0. + self.a_pid = 0. + self.last_output_gb = 0. + self.last_speed_kph = 0. + #for smoothing the changes in speed + self.v_acc_start = 0.0 + self.a_acc_start = 0.0 + self.acc_start_time = sec_since_boot() + self.v_acc = 0.0 + self.v_acc_sol = 0.0 + self.v_acc_future = 0.0 + self.a_acc = 0.0 + self.a_acc_sol = 0.0 + self.v_cruise = 0.0 + self.a_cruise = 0.0 + #Long Control + self.LoC = None + #when was radar data last updated? + self.last_md_ts = None + self.last_l100_ts = None + self.md_ts = None + self.l100_ts = None + self.lead_last_seen_time_ms = 0 + self.continuous_lead_sightings = 0 + + def reset(self, v_pid): + if self.LoC: + self.LoC.reset(v_pid) + + def update_stat(self, CS, enabled, sendcan): + if not self.LoC: + self.LoC = LongControl(CS.CP, tesla_compute_gb) + + can_sends = [] + if CS.pedal_interceptor_available and not CS.cstm_btns.get_button_status("pedal"): + # pedal hardware, enable button + CS.cstm_btns.set_button_status("pedal", 1) + print "enabling pedal" + elif not CS.pedal_interceptor_available: + if CS.cstm_btns.get_button_status("pedal"): + # no pedal hardware, disable button + CS.cstm_btns.set_button_status("pedal", 0) + print "disabling pedal" + print "Pedal unavailable." + return + + # check if we had error before + if self.pedal_interceptor_state != CS.pedal_interceptor_state: + self.pedal_interceptor_state = CS.pedal_interceptor_state + CS.cstm_btns.set_button_status("pedal", 1 if self.pedal_interceptor_state > 0 else 0) + if self.pedal_interceptor_state > 0: + # send reset command + idx = self.pedal_idx + self.pedal_idx = (self.pedal_idx + 1) % 16 + can_sends.append(teslacan.create_pedal_command_msg(0, 0, idx)) + sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) + CS.UE.custom_alert_message(3, "Pedal Interceptor Off (state %s)" % self.pedal_interceptor_state, 150, 3) + else: + CS.UE.custom_alert_message(3, "Pedal Interceptor On", 150, 3) + # disable on brake + if CS.brake_pressed and self.enable_pedal_cruise: + self.enable_pedal_cruise = False + self.reset(0.) + CS.UE.custom_alert_message(3, "PDL Disabled", 150, 4) + CS.cstm_btns.set_button_status("pedal", 1) + print "brake pressed" + + prev_enable_pedal_cruise = self.enable_pedal_cruise + # process any stalk movement + curr_time_ms = _current_time_millis() + speed_uom_kph = 1. + if CS.imperial_speed_units: + speed_uom_kph = CV.MPH_TO_KPH + if (CS.cruise_buttons == CruiseButtons.MAIN and + self.prev_cruise_buttons != CruiseButtons.MAIN): + self.prev_stalk_pull_time_ms = self.stalk_pull_time_ms + self.stalk_pull_time_ms = curr_time_ms + double_pull = self.stalk_pull_time_ms - self.prev_stalk_pull_time_ms < STALK_DOUBLE_PULL_MS + ready = (CS.cstm_btns.get_button_status("pedal") > PCCState.OFF + and enabled + and (CruiseState.is_off(CS.pcm_acc_status)) or CS.forcePedalOverCC) + if ready and double_pull: + # A double pull enables ACC. updating the max ACC speed if necessary. + self.enable_pedal_cruise = True + self.LoC.reset(CS.v_ego) + # Increase PCC speed to match current, if applicable. + self.pedal_speed_kph = max(CS.v_ego * CV.MS_TO_KPH, self.pedal_speed_kph) + # Handle pressing the cancel button. + elif CS.cruise_buttons == CruiseButtons.CANCEL: + self.enable_pedal_cruise = False + self.pedal_speed_kph = 0. + self.stalk_pull_time_ms = 0 + self.prev_stalk_pull_time_ms = -1000 + # Handle pressing up and down buttons. + elif (self.enable_pedal_cruise + and CS.cruise_buttons != self.prev_cruise_buttons): + # Real stalk command while PCC is already enabled. Adjust the max PCC + # speed if necessary. + actual_speed_kph = CS.v_ego * CV.MS_TO_KPH + if CS.cruise_buttons == CruiseButtons.RES_ACCEL: + self.pedal_speed_kph = max(self.pedal_speed_kph, actual_speed_kph) + speed_uom_kph + elif CS.cruise_buttons == CruiseButtons.RES_ACCEL_2ND: + self.pedal_speed_kph = max(self.pedal_speed_kph, actual_speed_kph) + 5 * speed_uom_kph + elif CS.cruise_buttons == CruiseButtons.DECEL_SET: + self.pedal_speed_kph = min(self.pedal_speed_kph, actual_speed_kph) - speed_uom_kph + elif CS.cruise_buttons == CruiseButtons.DECEL_2ND: + self.pedal_speed_kph = min(self.pedal_speed_kph, actual_speed_kph) - 5 * speed_uom_kph + # Clip PCC speed between 0 and 170 KPH. + self.pedal_speed_kph = clip(self.pedal_speed_kph, MIN_PCC_V_KPH, MAX_PCC_V_KPH) + # If something disabled cruise control, disable PCC too + elif self.enable_pedal_cruise and CS.pcm_acc_status and not CS.forcePedalOverCC: + self.enable_pedal_cruise = False + # A single pull disables PCC (falling back to just steering). Wait some time + # in case a double pull comes along. + elif (self.enable_pedal_cruise + and curr_time_ms - self.stalk_pull_time_ms > STALK_DOUBLE_PULL_MS + and self.stalk_pull_time_ms - self.prev_stalk_pull_time_ms > STALK_DOUBLE_PULL_MS): + self.enable_pedal_cruise = False + + # Notify if PCC was toggled + if prev_enable_pedal_cruise and not self.enable_pedal_cruise: + CS.UE.custom_alert_message(3, "PCC Disabled", 150, 4) + CS.cstm_btns.set_button_status("pedal", PCCState.STANDBY) + elif self.enable_pedal_cruise and not prev_enable_pedal_cruise: + CS.UE.custom_alert_message(2, "PCC Enabled", 150) + CS.cstm_btns.set_button_status("pedal", PCCState.ENABLED) + + # Update the UI to show whether the current car state allows PCC. + if CS.cstm_btns.get_button_status("pedal") in [PCCState.STANDBY, PCCState.NOT_READY]: + if enabled and (CruiseState.is_off(CS.pcm_acc_status) or CS.forcePedalOverCC): + CS.cstm_btns.set_button_status("pedal", PCCState.STANDBY) + else: + CS.cstm_btns.set_button_status("pedal", PCCState.NOT_READY) + + # Update prev state after all other actions. + self.prev_cruise_buttons = CS.cruise_buttons + self.prev_pcm_acc_status = CS.pcm_acc_status + + def update_pdl(self, enabled, CS, frame, actuators, pcm_speed): + cur_time = sec_since_boot() + idx = self.pedal_idx + self.pedal_idx = (self.pedal_idx + 1) % 16 + if not CS.pedal_interceptor_available or not enabled: + return 0., 0, idx + # Alternative speed decision logic that uses the lead car's distance + # and speed more directly. + # Bring in the lead car distance from the Live20 feed + l20 = None + if enabled: + for socket, _ in self.poller.poll(0): + if socket is self.live20: + l20 = messaging.recv_one(socket) + break + if l20 is not None: + self.lead_1 = l20.live20.leadOne + if _is_present(self.lead_1): + self.lead_last_seen_time_ms = _current_time_millis() + self.continuous_lead_sightings += 1 + else: + self.continuous_lead_sightings = 0 + self.md_ts = l20.live20.mdMonoTime + self.l100_ts = l20.live20.l100MonoTime + + brake_max, accel_max = calc_cruise_accel_limits(CS, self.lead_1) + output_gb = 0 + #################################################################### + # this mode (Follow) uses the Follow logic created by JJ for ACC + # + # once the speed is detected we have to use our own PID to determine + # how much accel and break we have to do + #################################################################### + if PCCModes.is_selected(FollowMode(), CS.cstm_btns): + self.v_pid = self.calc_follow_speed_ms(CS) + # cruise speed can't be negative even is user is distracted + self.v_pid = max(self.v_pid, 0.) + + enabled = self.enable_pedal_cruise and self.LoC.long_control_state in [LongCtrlState.pid, LongCtrlState.stopping] + + if self.enable_pedal_cruise: + jerk_min, jerk_max = _jerk_limits(CS.v_ego, self.lead_1, self.pedal_speed_kph, self.lead_last_seen_time_ms) + self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start, + self.v_pid, + accel_max, brake_max, + jerk_max, jerk_min, + _DT_MPC) + + # cruise speed can't be negative even is user is distracted + self.v_cruise = max(self.v_cruise, 0.) + self.v_acc = self.v_cruise + self.a_acc = self.a_cruise + + self.v_acc_future = self.v_pid + + self.v_acc_start = self.v_acc_sol + self.a_acc_start = self.a_acc_sol + self.acc_start_time = cur_time + + # Interpolation of trajectory + dt = min(cur_time - self.acc_start_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps + self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start) + self.v_acc_sol = self.v_acc_start + dt * (self.a_acc_sol + self.a_acc_start) / 2.0 + + # we will try to feed forward the pedal position.... we might want to feed the last output_gb.... + # it's all about testing now. + vTarget = clip(self.v_acc_sol, 0, self.v_pid) + self.vTargetFuture = clip(self.v_acc_future, 0, self.v_pid) + + t_go, t_brake = self.LoC.update(self.enable_pedal_cruise, CS.v_ego, CS.brake_pressed != 0, CS.standstill, False, + self.v_pid , vTarget, self.vTargetFuture, self.a_acc_sol, CS.CP, None) + output_gb = t_go - t_brake + #print "Output GB Follow:", output_gb + else: + self.LoC.reset(CS.v_ego) + #print "PID reset" + output_gb = 0. + starting = self.LoC.long_control_state == LongCtrlState.starting + a_ego = min(CS.a_ego, 0.0) + reset_speed = MIN_CAN_SPEED if starting else CS.v_ego + reset_accel = CS.CP.startAccel if starting else a_ego + self.v_acc = reset_speed + self.a_acc = reset_accel + self.v_acc_start = reset_speed + self.a_acc_start = reset_accel + self.v_cruise = reset_speed + self.a_cruise = reset_accel + self.v_acc_sol = reset_speed + self.a_acc_sol = reset_accel + self.v_pid = reset_speed + + ############################################################## + # This mode uses the longitudinal MPC built in OP + # + # we use the values from actuator.accel and actuator.brake + ############################################################## + elif PCCModes.is_selected(OpMode(), CS.cstm_btns): + output_gb = actuators.gas - actuators.brake + + ###################################################################################### + # Determine pedal "zero" + # + #save position for cruising (zero acc, zero brake, no torque) when we are above 10 MPH + ###################################################################################### + if (CS.torqueLevel < TORQUE_LEVEL_ACC + and CS.torqueLevel > TORQUE_LEVEL_DECEL + and CS.v_ego >= 10.* CV.MPH_TO_MS + and abs(CS.torqueLevel) < abs(self.lastTorqueForPedalForZeroTorque)): + self.PedalForZeroTorque = self.prev_tesla_accel + self.lastTorqueForPedalForZeroTorque = CS.torqueLevel + #print "Detected new Pedal For Zero Torque at %s" % (self.PedalForZeroTorque) + #print "Torque level at detection %s" % (CS.torqueLevel) + #print "Speed level at detection %s" % (CS.v_ego * CV.MS_TO_MPH) + + self.last_output_gb = output_gb + # accel and brake + apply_accel = clip(output_gb, 0., accel_max) + MPC_BRAKE_MULTIPLIER = 6. + apply_brake = -clip(output_gb * MPC_BRAKE_MULTIPLIER, -brake_max, 0.) + + # if speed is over 5mpg, the "zero" is at PedalForZeroTorque; otherwise it is zero + pedal_zero = 0. + if CS.v_ego >= 5.* CV.MPH_TO_MS: + pedal_zero = self.PedalForZeroTorque + tesla_brake = clip((1. - apply_brake) * pedal_zero, 0, pedal_zero) + tesla_accel = clip(apply_accel * MAX_PEDAL_VALUE, 0, MAX_PEDAL_VALUE - tesla_brake) + tesla_pedal = tesla_brake + tesla_accel + + tesla_pedal = self.pedal_hysteresis(tesla_pedal, enabled) + + tesla_pedal = clip(tesla_pedal, self.prev_tesla_pedal - PEDAL_MAX_DOWN, self.prev_tesla_pedal + PEDAL_MAX_UP) + tesla_pedal = clip(tesla_pedal, 0., MAX_PEDAL_VALUE) if self.enable_pedal_cruise else 0. + enable_pedal = 1. if self.enable_pedal_cruise else 0. + + self.torqueLevel_last = CS.torqueLevel + self.prev_tesla_pedal = tesla_pedal * enable_pedal + self.prev_tesla_accel = apply_accel * enable_pedal + self.prev_v_ego = CS.v_ego + + self.last_md_ts = self.md_ts + self.last_l100_ts = self.l100_ts + + return self.prev_tesla_pedal, enable_pedal, idx + + # function to calculate the cruise speed based on a safe follow distance + def calc_follow_speed_ms(self, CS): + # Make sure we were able to populate lead_1. + if self.lead_1 is None: + return None, None, None + # dRel is in meters. + lead_dist_m = _visual_radar_adjusted_dist_m(self.lead_1.dRel) + # Grab the relative speed. + rel_speed_kph = self.lead_1.vRel * CV.MS_TO_KPH + # v_ego is in m/s, so safe_distance is in meters. + safe_dist_m = _safe_distance_m(CS.v_ego) + # Current speed in kph + actual_speed_kph = CS.v_ego * CV.MS_TO_KPH + # speed and brake to issue + new_speed_kph = self.last_speed_kph + ### Logic to determine best cruise speed ### + if self.enable_pedal_cruise: + # If no lead is present, accel up to max speed + if lead_dist_m == 0: + new_speed_kph = self.pedal_speed_kph + elif lead_dist_m > 0: + lead_absolute_speed_kph = actual_speed_kph + rel_speed_kph + if lead_dist_m < MIN_SAFE_DIST_M: + new_speed_kph = MIN_PCC_V_KPH + else: + # Force speed into a band that is generally slower than lead if too + # close, and faster than lead if too far. Allow a range of speeds at + # any given distance, to prevent continuous jerky adjustments. + min_vrel_kph_map = OrderedDict([ + # (distance in m, min allowed relative kph) + (0.5 * safe_dist_m, 2), + (1.0 * safe_dist_m, -6), + (1.5 * safe_dist_m, -10), + (3.0 * safe_dist_m, -20)]) + min_vrel_kph = _interp_map(lead_dist_m, min_vrel_kph_map) + max_vrel_kph_map = OrderedDict([ + # (distance in m, max allowed relative kph) + (0.5 * safe_dist_m, 8), + (1.0 * safe_dist_m, 1), + (1.5 * safe_dist_m, 0), + # With visual radar the relative velocity is 0 until the confidence + # gets high. So even a small negative number here gives constant + # accel until lead lead car gets close enough to read. + (2.0 * safe_dist_m, -2)]) + max_vrel_kph = _interp_map(lead_dist_m, max_vrel_kph_map) + min_kph = lead_absolute_speed_kph - max_vrel_kph + max_kph = lead_absolute_speed_kph - min_vrel_kph + # In the special case were we are going faster than intended but it's + # still an acceptable speed, accept it. This could happen if the + # driver manually accelerates, or if we roll down a hill. In either + # case, don't fight the extra velocity unless necessary. + if actual_speed_kph > new_speed_kph and min_kph < actual_speed_kph < max_kph: + new_speed_kph = actual_speed_kph + + new_speed_kph = clip(new_speed_kph, min_kph, max_kph) + + # Enforce limits on speed in the presence of a lead car. + new_speed_kph = min(new_speed_kph, + _max_safe_speed_kph(lead_dist_m), + lead_absolute_speed_kph - _min_safe_vrel_kph(lead_dist_m)) + + # Enforce limits on speed + new_speed_kph = clip(new_speed_kph, MIN_PCC_V_KPH, MAX_PCC_V_KPH) + new_speed_kph = clip(new_speed_kph, MIN_PCC_V_KPH, self.pedal_speed_kph) + if CS.blinker_on: + # Don't accelerate during manual turns. + new_speed_kph = min(new_speed_kph, self.last_speed_kph) + self.last_speed_kph = new_speed_kph + + return new_speed_kph * CV.KPH_TO_MS + + def pedal_hysteresis(self, pedal, enabled): + # for small accel oscillations within PEDAL_HYST_GAP, don't change the command + if not enabled: + # send 0 when disabled, otherwise acc faults + self.pedal_steady = 0. + elif pedal > self.pedal_steady + PEDAL_HYST_GAP: + self.pedal_steady = pedal - PEDAL_HYST_GAP + elif pedal < self.pedal_steady - PEDAL_HYST_GAP: + self.pedal_steady = pedal + PEDAL_HYST_GAP + return self.pedal_steady + +def _visual_radar_adjusted_dist_m(m): + # visual radar sucks at short distances. It rarely shows readings below 7m. + # So rescale distances with 7m -> 0m. Maxes out at 1km, if that matters. + mapping = OrderedDict([ + # (input distance, output distance) + (7, 0), # anything below 7m is set to 0m. + (1000, 1000)]) # values >7m are scaled, maxing out at 1km. + return _interp_map(m, mapping) + +def _safe_distance_m(v_ego_ms): + return max(FOLLOW_TIME_S * v_ego_ms, MIN_SAFE_DIST_M) + +def _max_safe_speed_kph(m): + return CV.MS_TO_KPH * m / FOLLOW_TIME_S + +def _min_safe_vrel_kph(m): + min_vrel_by_distance = OrderedDict([ + # (meters, safe relative velocity in kph) + # Remember, a negative relative velocity means we are closing the distance. + (MIN_SAFE_DIST_M, 1), # If lead is close, it better be pulling away. + (200, -15)]) # if lead is far, it's ok if we're closing. + return _interp_map(m, min_vrel_by_distance) + +def _is_present(lead): + return bool(lead and lead.dRel) + +def _sec_til_collision(lead): + if _is_present(lead) and lead.vRel < 0: + return _visual_radar_adjusted_dist_m(lead.dRel) / abs(lead.vRel) + else: + return 60 # Arbitrary, but better than MAXINT because we can still do math on it. + +def _interp_map(val, val_map): + """Helper to call interp with an OrderedDict for the mapping. I find + this easier to read than interp, which takes two arrays.""" + return interp(val, val_map.keys(), val_map.values()) + +def _accel_limit_multiplier(v_ego, lead): + """Limits acceleration in the presence of a lead car. The further the lead car + is, the more accel is allowed. Range: 0 to 1, so that it can be multiplied + with other accel limits.""" + if _is_present(lead): + safe_dist_m = _safe_distance_m(v_ego) + accel_multipliers = OrderedDict([ + # (distance in m, acceleration fraction) + (0.6 * safe_dist_m, 0.0), + (1.0 * safe_dist_m, 0.4), + (3.0 * safe_dist_m, 1.0)]) + return _interp_map(lead.dRel, accel_multipliers) + else: + return 1.0 + +def _decel_limit_multiplier(v_ego, lead): + if _is_present(lead): + decel_map = OrderedDict([ + # (sec to collision, decel) + (2, 1.0), + (4, 0.4), + (8, 0.1)]) + return _interp_map(_sec_til_collision(lead), decel_map) + else: + return 1.0 + +def _jerk_limits(v_ego, lead, max_speed_kph, lead_last_seen_time_ms): + # prevent high accel jerk near max speed + near_max_speed_multipliers = OrderedDict([ + # (kph under max speed, accel jerk multiplier) + (0, 0.01), + (4, 1.0)]) + near_max_speed_multiplier = _interp_map(max_speed_kph - v_ego * CV.MS_TO_KPH, near_max_speed_multipliers) + + if _is_present(lead): + # pick decel jerk based on how much time we have til collision + decel_jerk_map = OrderedDict([ + # (sec to collision, decel jerk) + (0, -1.00), + (2, -0.25), + (4, -0.01), + (8, -0.001)]) + decel_jerk = _interp_map(_sec_til_collision(lead), decel_jerk_map) + safe_dist_m = _safe_distance_m(v_ego) + accel_jerk_map = OrderedDict([ + # (distance in m, accel jerk) + (0.8 * safe_dist_m, 0.01), + (2.8 * safe_dist_m, 0.11)]) + accel_jerk = _interp_map(lead.dRel, accel_jerk_map) + accel_jerk *= near_max_speed_multiplier + return decel_jerk, accel_jerk + else: + # In the absence of a lead car + decel_jerk = -0.15 + # Limit accel jerk if the lead was only recently lost, to prevent + # bucking as a lead is intermittently detected. + time_since_lead_seen_ms = _current_time_millis() - lead_last_seen_time_ms + time_since_lead_seen_multipliers = OrderedDict([ + # (ms since last lead sighting, accel jerk multiplier) + (0, 0.1), + (2000, 1.0)]) + time_since_lead_seen_multiplier = _interp_map(time_since_lead_seen_ms, time_since_lead_seen_multipliers) + # Allow higher jerk at low speed, to get started + accel_jerk_by_speed = OrderedDict([ + # (Speed in m/s, accel jerk) + (0, 0.18), + (12, 0.10)]) + accel_jerk = _interp_map(v_ego, accel_jerk_by_speed) + accel_jerk *= near_max_speed_multiplier * time_since_lead_seen_multiplier + return decel_jerk, accel_jerk diff --git a/selfdrive/car/tesla/__init__.py b/selfdrive/car/tesla/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py new file mode 100644 index 00000000000000..47e51e125c6cdd --- /dev/null +++ b/selfdrive/car/tesla/carcontroller.py @@ -0,0 +1,334 @@ +import os +import subprocess +from threading import Thread +import traceback +import shlex +from common.params import Params +from collections import namedtuple +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, CM +from selfdrive.can.packer import CANPacker +from selfdrive.config import Conversions as CV +from selfdrive.car.modules.ALCA_module import ALCAController +from selfdrive.car.tesla.ACC_module import ACCController +from selfdrive.car.tesla.PCC_module import PCCController +from selfdrive.car.tesla.HSO_module import HSOController +import zmq +import selfdrive.messaging as messaging +from selfdrive.services import service_list + +# 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, .25] # windup limit +ANGLE_DELTA_VU = [5., 3.5, 0.8] # unwind limit +#steering adjustment with speed +DES_ANGLE_ADJUST_FACTOR_BP = [0.,13., 44.] +DES_ANGLE_ADJUST_FACTOR = [.70, .80, .99] + +def process_hud_alert(hud_alert): + # initialize to no alert + fcw_display = 0 + steer_required = 0 + acc_alert = 0 + if hud_alert == AH.NONE: # no alert + pass + elif hud_alert == AH.FCW: # FCW + fcw_display = hud_alert[1] + elif hud_alert == AH.STEER: # STEER + steer_required = hud_alert[1] + else: # any other ACC alert + acc_alert = hud_alert[1] + + return fcw_display, steer_required, acc_alert + + +HUDData = namedtuple("HUDData", + ["pcm_accel", "v_cruise", "mini_car", "car", "X4", + "lanes", "beep", "chime", "fcw", "acc_alert", "steer_required"]) + + +class CarController(object): + def __init__(self, dbc_name, enable_camera=True): + self.braking = False + self.brake_steady = 0. + self.brake_last = 0. + self.enable_camera = enable_camera + self.packer = CANPacker(dbc_name) + self.epas_disabled = True + self.last_angle = 0. + self.last_accel = 0. + self.ALCA = ALCAController(self,True,True) # Enabled and SteerByAngle both True + self.ACC = ACCController() + self.PCC = PCCController(self) + self.HSO = HSOController(self) + self.sent_DAS_bootID = False + context = zmq.Context() + self.poller = zmq.Poller() + self.speedlimit = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True, poller=self.poller) + self.speedlimit_ms = 0 + self.speedlimit_valid = False + self.speedlimit_units = 0 + self.opState = 0 # 0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning + + def update(self, sendcan, enabled, CS, frame, actuators, \ + pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ + hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \ + snd_beep, snd_chime): + + """ Controls thread """ + + ## Todo add code to detect Tesla DAS (camera) and go into listen and record mode only (for AP1 / AP2 cars) + if not self.enable_camera: + return + + # *** no output if not enabled *** + if not enabled and CS.pcm_acc_status: + # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated + pcm_cancel_cmd = True + + # vehicle hud display, wait for one update from 10Hz 0x304 msg + if hud_show_lanes: + hud_lanes = 1 + else: + hud_lanes = 0 + + # TODO: factor this out better + if enabled: + if hud_show_car: + hud_car = 2 + else: + hud_car = 1 + else: + hud_car = 0 + + # For lateral control-only, send chimes as a beep since we don't send 0x1fa + #if CS.CP.radarOffCan: + + #print chime, alert_id, hud_alert + fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) + + hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car, + 0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required) + + if not all(isinstance(x, int) and 0 <= x < 256 for x in hud): + print "INVALID HUD", hud + hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0) + + # **** process the car messages **** + + # *** compute control surfaces *** + + STEER_MAX = 420 + # Prevent steering while stopped + MIN_STEERING_VEHICLE_VELOCITY = 0.05 # m/s + vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY) + + # Basic highway lane change logic + changing_lanes = CS.right_blinker_on or CS.left_blinker_on + + #upodate custom UI buttons and alerts + CS.UE.update_custom_ui() + + if (frame % 1000 == 0): + CS.cstm_btns.send_button_info() + + # Update statuses for custom buttons every 0.1 sec. + if self.ALCA.pid == None: + self.ALCA.set_pid(CS) + if (frame % 10 == 0): + self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0 and CS.enableALCA) + #print CS.cstm_btns.get_button_status("alca") + + + if CS.pedal_interceptor_available: + #update PCC module info + self.PCC.update_stat(CS, True, sendcan) + self.ACC.enable_adaptive_cruise = False + else: + # Update ACC module info. + self.ACC.update_stat(CS, True) + self.PCC.enable_pedal_cruise = False + + # 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 + alca_steer = 0. + apply_angle, alca_steer,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) + human_lane_changing = changing_lanes and not alca_enabled + enable_steer_control = (enabled + and not human_lane_changing + and not human_control + and vehicle_moving) + + angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V) + apply_angle = clip(apply_angle, -angle_lim, angle_lim) + # Windup slower. + if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): + angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V) + else: + angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU) + + des_angle_factor = interp(CS.v_ego, DES_ANGLE_ADJUST_FACTOR_BP, DES_ANGLE_ADJUST_FACTOR ) + if alca_enabled or not CS.enableSpeedVariableDesAngle: + des_angle_factor = 1. + apply_angle = clip(apply_angle * des_angle_factor, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) + # If human control, send the steering angle as read at steering wheel. + if human_control: + apply_angle = CS.angle_steers + + # Send CAN commands. + can_sends = [] + + #First we emulate DAS. + # DAS_longC_enabled (1),DAS_gas_to_resume (1),DAS_apUnavailable (1), DAS_collision_warning (1), DAS_op_status (4) + # DAS_speed_kph(8), + # DAS_turn_signal_request (2),DAS_forward_collision_warning (2), DAS_hands_on_state (4), + # DAS_cc_state (2), DAS_usingPedal(1),DAS_alca_state (5), + # DAS_acc_speed_limit_mph (8), + # DAS_speed_limit_units(8) + #send fake_das data as 0x553 + # TODO: forward collission warning + if frame % 10 == 0: + #get speed limit + for socket, _ in self.poller.poll(0): + if socket is self.speedlimit: + lmd = messaging.recv_one(socket).liveMapData + self.speedlimit_ms = lmd.speedLimit + self.speedlimit_valid = lmd.speedLimitValid + params = Params() + if (params.get("IsMetric") == "1"): + self.speedlimit_units = self.speedlimit_ms * CV.MS_TO_KPH + 0.5 + else: + self.speedlimit_units = self.speedlimit_ms * CV.MS_TO_MPH + 0.5 + op_status = 0x02 + hands_on_state = 0x00 + forward_collision_warning = 0 #1 if needed + if hud_alert == AH.FCW: + forward_collision_warning = hud_alert[1] + if forward_collision_warning > 1: + forward_collision_warning = 1 + #cruise state: 0 unavailable, 1 available, 2 enabled, 3 hold + cc_state = 1 + speed_limit_to_car = int(self.speedlimit_units) + alca_state = 0x00 + apUnavailable = 0 + gas_to_resume = 0 + collision_warning = 0x00 + acc_speed_limit_mph = 0 + speed_control_enabled = 0 + accel_min = -15 + accel_max = 5 + acc_speed_kph = 0 + if enabled: + #self.opState 0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning + if self.opState == 0: + op_status = 0x02 + if self.opState == 1: + op_status = 0x03 + if self.opState == 2: + op_status = 0x08 + if self.opState == 3: + op_status = 0x01 + if self.opState == 5: + op_status = 0x03 + alca_state = 0x08 + turn_signal_needed + #canceled by user + if self.ALCA.laneChange_cancelled and (self.ALCA.laneChange_cancelled_counter > 0): + alca_state = 0x14 + #min speed for ALCA + if CS.CL_MIN_V > CS.v_ego: + alca_state = 0x05 + if not enable_steer_control: + #op_status = 0x08 + hands_on_state = 0x02 + apUnavailable = 1 + if hud_alert == AH.STEER: + if snd_chime == CM.MUTE: + hands_on_state = 0x03 + else: + hands_on_state = 0x05 + acc_speed_limit_mph = max(self.ACC.acc_speed_kph * CV.KPH_TO_MPH,1) + if CS.pedal_interceptor_available: + acc_speed_limit_mph = max(self.PCC.pedal_speed_kph * CV.KPH_TO_MPH,1) + if hud_alert == AH.FCW: + collision_warning = hud_alert[1] + if collision_warning > 1: + collision_warning = 1 + #use disabling for alerts/errors to make them aware someting is goin going on + if (snd_chime == CM.DOUBLE) or (hud_alert == AH.FCW): + op_status = 0x08 + if self.ACC.enable_adaptive_cruise: + acc_speed_kph = self.ACC.new_speed #pcm_speed * CV.MS_TO_KPH + if (CS.pedal_interceptor_available and self.PCC.enable_pedal_cruise) or (self.ACC.enable_adaptive_cruise): + speed_control_enabled = 1 + cc_state = 2 + else: + if (CS.pcm_acc_status == 4): + #car CC enabled but not OP, display the HOLD message + cc_state = 3 + send_fake_msg = False + send_fake_warning = False + if enabled: + if frame % 2 == 0: + send_fake_msg = True + if frame % 25 == 0: + send_fake_warning = True + else: + if frame % 23 == 0: + send_fake_msg = True + if frame % 60 == 0: + send_fake_warning = True + if send_fake_msg: + can_sends.append(teslacan.create_fake_DAS_msg(speed_control_enabled,gas_to_resume,apUnavailable, collision_warning, op_status, \ + acc_speed_kph, \ + turn_signal_needed,forward_collision_warning,hands_on_state, \ + cc_state, 1 if (CS.pedal_interceptor_available) else 0,alca_state, \ + acc_speed_limit_mph, + speed_limit_to_car, + apply_angle, + 1 if enable_steer_control else 0)) + if send_fake_warning or (self.opState == 2) or (self.opState == 5): + #if it's time to send OR we have a warning or emergency disable + can_sends.append(teslacan.create_fake_DAS_warning(CS.DAS_noSeatbelt, CS.DAS_canErrors, \ + CS.DAS_plannerErrors, CS.DAS_doorOpen, CS.DAS_notInDrive, CS.enableDasEmulation, CS.enableRadarEmulation)) + # end of DAS emulation """ + + idx = frame % 16 + cruise_btn = None + if self.ACC.enable_adaptive_cruise and not CS.pedal_interceptor_available: + cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators, pcm_speed) + if cruise_btn: + cruise_msg = teslacan.create_cruise_adjust_msg( + spdCtrlLvr_stat=cruise_btn, + turnIndLvr_Stat= 0, #turn_signal_needed, + real_steering_wheel_stalk=CS.steering_wheel_stalk) + # Send this CAN msg first because it is racing against the real stalk. + can_sends.insert(0, cruise_msg) + apply_accel = 0. + if CS.pedal_interceptor_available and frame % 5 == 0: # pedal processed at 20Hz + apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(enabled, CS, frame, actuators, pcm_speed) + can_sends.append(teslacan.create_pedal_command_msg(apply_accel, int(accel_needed), accel_idx)) + self.last_angle = apply_angle + self.last_accel = apply_accel + 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 new file mode 100644 index 00000000000000..1e0bf5c99181f3 --- /dev/null +++ b/selfdrive/car/tesla/carstate.py @@ -0,0 +1,535 @@ +from common.numpy_fast import interp +from common.kalman.simple_kalman import KF1D +from selfdrive.can.parser import CANParser +from selfdrive.config import Conversions as CV +from selfdrive.car.tesla.ACC_module import ACCMode +from selfdrive.car.tesla.PCC_module import PCCModes +from selfdrive.car.tesla.values import CAR, CruiseButtons, DBC +from selfdrive.car.modules.UIBT_module import UIButtons, UIButton +import numpy as np +from ctypes import create_string_buffer +from selfdrive.car.modules.UIEV_module import UIEvents +from selfdrive.car.tesla.readconfig import read_config_file +import os +import subprocess +import sys + +def parse_gear_shifter(can_gear_shifter, car_fingerprint): + + # TODO: Use VAL from DBC to parse this field + if car_fingerprint == CAR.MODELS: + # VAL_ 280 DI_gear 4 "D" 0 "INVALID" 3 "N" 1 "P" 2 "R" 7 "SNA" ; + if can_gear_shifter == 0x1: + return "park" + elif can_gear_shifter == 0x2: + return "reverse" + elif can_gear_shifter == 0x3: + return "neutral" + elif can_gear_shifter == 0x4: + return "drive" + + 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: + # - speed = 0m/s, out = -0.3 + # - speed = 34m/s, offset = 20, out = -0.25 + # - speed = 34m/s, offset = -2.5, out = -1.8 + _K0 = -0.3 + _K1 = -0.01879 + _K2 = 0.01013 + return min(_K0 + _K1 * speed + _K2 * speed * offset, 0.) + + +def get_can_signals(CP): +# this function generates lists for signal, messages and initial values + signals = [ + ("MCU_gpsVehicleHeading", "MCU_gpsVehicleSpeed", 0), + ("MCU_gpsAccuracy", "MCU_locationStatus", 0), + ("MCU_latitude", "MCU_locationStatus", 0), + ("MCU_longitude", "MCU_locationStatus", 0), + ("DI_vehicleSpeed", "DI_torque2", 0), + ("StW_AnglHP", "STW_ANGLHP_STAT", 0), + ("EPAS_torsionBarTorque", "EPAS_sysStatus", 0), # Used in interface.py + ("EPAS_eacStatus", "EPAS_sysStatus", 0), + ("EPAS_handsOnLevel", "EPAS_sysStatus", 0), + ("EPAS_steeringFault", "EPAS_sysStatus", 0), + ("DI_gear", "DI_torque2", 3), + ("DOOR_STATE_FL", "GTW_carState", 1), + ("DOOR_STATE_FR", "GTW_carState", 1), + ("DOOR_STATE_RL", "GTW_carState", 1), + ("DOOR_STATE_RR", "GTW_carState", 1), + ("YEAR", "GTW_carState", 1), # Added for debug + ("MONTH", "GTW_carState", 1), # Added for debug + ("DAY", "GTW_carState", 1), # Added for debug + ("MINUTE", "GTW_carState", 1), # Added for debug + ("HOUR", "GTW_carState", 1), # Added for debug + ("SECOND", "GTW_carState", 1), # Added for debug + # Added for debug ---below + ("DI_torque2Counter", "DI_torque2", 0), + ("DI_brakePedalState", "DI_torque2", 0), + ("DI_epbParkRequest", "DI_torque2", 0), + ("DI_epbInterfaceReady", "DI_torque2", 0), + ("DI_torqueEstimate", "DI_torque2", 0), + # Added for debug --above + ("DI_stateCounter", "DI_state", 0), + ("GTW_driverPresent", "GTW_status", 0), + ("DI_brakePedal", "DI_torque2", 0), + ("DI_cruiseSet", "DI_state", 0), + ("DI_cruiseState", "DI_state", 0), + ("TSL_P_Psd_StW","SBW_RQ_SCCM" , 0), + ("DI_motorRPM", "DI_torque1", 0), + ("DI_pedalPos", "DI_torque1", 0), + ("DI_torqueMotor", "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), + ("DI_regenLight", "DI_state",0), + ("GTW_performanceConfig", "GTW_carConfig",0), + ("GTW_fourWheelDrive", "GTW_carConfig",0), + ("GTW_unknown1", "GTW_carConfig",0), + ("GTW_dasHw", "GTW_carConfig",0), + ("GTW_parkAssistInstalled", "GTW_carConfig",0), + ("GTW_forwardRadarHw", "GTW_carConfig",0), + ("GTW_airSuspensionInstalled", "GTW_carConfig",0), + ("GTW_unknown2", "GTW_carConfig",0), + ("GTW_country", "GTW_carConfig",0), + ("GTW_parkSensorGeometryType", "GTW_carConfig",0), + ("GTW_rhd", "GTW_carConfig",0), + ("GTW_bodyControlsType", "GTW_carConfig",0), + ("GTW_radarPosition", "GTW_carConfig",0), + ("GTW_rearCornerRadarHw", "GTW_carConfig",0), + ("GTW_frontCornerRadarHw", "GTW_carConfig",0), + ("GTW_epasType", "GTW_carConfig",0), + ("GTW_chassisType", "GTW_carConfig",0), + ("GTW_wheelType", "GTW_carConfig",0), + ("GTW_rearSeatControllerMask", "GTW_carConfig",0), + ("GTW_euVehicle", "GTW_carConfig",0), + ("GTW_foldingMirrorsInstalled", "GTW_carConfig",0), + ("GTW_brakeHwType", "GTW_carConfig",0), + ("GTW_autopilot", "GTW_carConfig",0), + ("GTW_unknown3", "GTW_carConfig",0), + ("SDM_bcklDrivStatus", "SDM1", 0), + + ] + + checks = [ + ("STW_ANGLHP_STAT", 200), #JCT Actual message freq is 40 Hz (0.025 sec) + ("STW_ACTN_RQ", 17), #JCT Actual message freq is 3.5 Hz (0.285 sec) + ("SBW_RQ_SCCM", 175), #JCT Actual message freq is 35 Hz (0.0286 sec) + ("DI_torque1", 59), #JCT Actual message freq is 11.8 Hz (0.084 sec) + ("DI_torque2", 18), #JCT Actual message freq is 3.7 Hz (0.275 sec) + ("MCU_gpsVehicleSpeed", 2), #JCT Actual message freq is 0.487 Hz (2.05 sec) + ("GTW_carState", 16), #JCT Actual message freq is 3.3 Hz (0.3 sec) + ("GTW_status", 2), #JCT Actual message freq is 0.5 Hz (2 sec) + ("DI_state", 5), #JCT Actual message freq is 1 Hz (1 sec) + ("EPAS_sysStatus", 0), #JCT Actual message freq is 1.3 Hz (0.76 sec) + ("MCU_locationStatus", 5), #JCT Actual message freq is 1.3 Hz (0.76 sec) + #("GTW_carConfig", 5), #BB Actual message freq is 1 Hz (1 sec) + ] + + #checks = [] + return signals, checks + +def get_epas_can_signals(CP): +# this function generates lists for signal, messages and initial values + signals = [ + ("EPAS_torsionBarTorque", "EPAS_sysStatus", 0), # Used in interface.py + ("EPAS_eacStatus", "EPAS_sysStatus", 0), + ("EPAS_eacErrorCode", "EPAS_sysStatus", 0), + ("EPAS_handsOnLevel", "EPAS_sysStatus", 0), + ("EPAS_steeringFault", "EPAS_sysStatus", 0), + ("EPAS_internalSAS", "EPAS_sysStatus", 0), #BB see if this works better than STW_ANGLHP_STAT for angle + ("INTERCEPTOR_GAS", "GAS_SENSOR", 0), + ("INTERCEPTOR_GAS2", "GAS_SENSOR", 0), + ("STATE", "GAS_SENSOR", 0), + ("IDX", "GAS_SENSOR", 0), + ] + + checks = [ + ("EPAS_sysStatus", 5), #JCT Actual message freq is 1.3 Hz (0.76 sec) + #("GAS_SENSOR", 50), # BB Actual message freq is 10 Hz (0.1 sec) + ] + + #checks = [] + return signals, checks + +def get_can_parser(CP): + signals, checks = get_can_signals(CP) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) + +def get_epas_parser(CP): + signals, checks = get_epas_can_signals(CP) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) + + +class CarState(object): + def __init__(self, CP): + # labels for buttons + self.btns_init = [["alca", "ALC", ["MadMax", "Normal", "Calm"]], + [ACCMode.BUTTON_NAME, ACCMode.BUTTON_ABREVIATION, ACCMode.labels()], + ["tsk", "TSK", ["Left","Middle","Right"]], + ["vision", "VIS", ["wiggly","normal"]], + ["msg", "MSG", [""]], + ["sound", "SND", [""]]] + + ### START OF MAIN CONFIG OPTIONS ### + ### Do NOT modify here, modify in /data/bb_openpilot.cfg and reboot + self.forcePedalOverCC = True + self.enableHSO = True + self.enableALCA = True + self.enableDasEmulation = True + self.enableRadarEmulation = True + self.enableSpeedVariableDesAngle = True + #read config file + read_config_file(self) + ### END OF MAIN CONFIG OPTIONS ### + + + + if (CP.carFingerprint == CAR.MODELS): + # ALCA PARAMS + + # max REAL delta angle for correction vs actuator + self.CL_MAX_ANGLE_DELTA_BP = [10., 44.] + self.CL_MAX_ANGLE_DELTA = [2.2, .3] + + # adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother + self.CL_ADJUST_FACTOR_BP = [10., 44.] + self.CL_ADJUST_FACTOR = [16. , 8.] + + + # reenrey angle when to let go + self.CL_REENTRY_ANGLE_BP = [10., 44.] + self.CL_REENTRY_ANGLE = [5. , 5.] + + # a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line + self.CL_LANE_DETECT_BP = [10., 44.] + self.CL_LANE_DETECT_FACTOR = [1.5, 1.5] + + self.CL_LANE_PASS_BP = [10., 20., 44.] + self.CL_LANE_PASS_TIME = [40.,20., 13.] + + # change lane delta angles and other params + self.CL_MAXD_BP = [10., 32., 44.] + self.CL_MAXD_A = [.358, 0.084, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75 + + self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s + + # do not turn if actuator wants more than x deg for going straight; this should be interp based on speed + self.CL_MAX_A_BP = [10., 44.] + self.CL_MAX_A = [10., 10.] + + # define limits for angle change every 0.1 s + # we need to force correction above 10 deg but less than 20 + # anything more means we are going to steep or not enough in a turn + self.CL_MAX_ACTUATOR_DELTA = 2. + self.CL_MIN_ACTUATOR_DELTA = 0. + self.CL_CORRECTION_FACTOR = 1. + + #duration after we cross the line until we release is a factor of speed + self.CL_TIMEA_BP = [10., 32., 44.] + self.CL_TIMEA_T = [0.7 ,0.50, 0.40] + + #duration to wait (in seconds) with blinkers on before starting to turn + self.CL_WAIT_BEFORE_START = 1 + + #END OF ALCA PARAMS + + + + self.brake_only = CP.enableCruise + self.last_cruise_stalk_pull_time = 0 + self.CP = CP + + self.user_gas = 0. + self.user_gas_pressed = False + self.pedal_interceptor_state = 0 + self.pedal_interceptor_value = 0. + self.pedal_interceptor_value2 = 0. + self.pedal_interceptor_missed_counter = 0 + self.brake_switch_prev = 0 + self.brake_switch_ts = 0 + + self.cruise_buttons = 0 + self.blinker_on = 0 + + self.left_blinker_on = 0 + self.right_blinker_on = 0 + self.steer_warning = 0 + + self.stopped = 0 + + # 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 = 1 + self.DAS_lanes_idx = 1 + self.DAS_objects_frm = 0 + self.DAS_objects_idx = 0 + self.DAS_pscControl_frm = 0 + self.DAS_pscControl_idx = 0 + self.DAS_warningMatrix0_idx = 0 + self.DAS_warningMatrix1_idx = 0 + self.DAS_warningMatrix3_idx = 0 + self.DAS_telemetryPeriodic1_idx = 0 + self.DAS_telemetryPeriodic2_idx = 0 + self.DAS_telemetryEvent1_idx = 0 + self.DAS_telemetryEvent2_idx = 0 + self.DAS_control_idx = 0 + + #BB notification messages for DAS + self.DAS_noSeatbelt = 0 + self.DAS_canErrors = 0 + self.DAS_plannerErrors = 0 + self.DAS_doorOpen = 0 + self.DAS_notInDrive = 0 + + #BB variables for pedal CC + self.pedal_speed_kph = 0. + # Pedal mode is ready, i.e. hardware is present and normal cruise is off. + self.pedal_interceptor_available = False + self.prev_pedal_interceptor_available = False + + #BB UIEvents + self.UE = UIEvents(self) + + #BB PCC + self.regenLight = 0 + self.torqueLevel = 0. + + #BB variable for custom buttons + self.cstm_btns = UIButtons(self,"Tesla Model S","tesla") + + #BB custom message counter + self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero + + #BB steering_wheel_stalk last position, used by ACC and ALCA + self.steering_wheel_stalk = None + + #BB carConfig data used to change IC info + self.real_carConfig = None + self.real_dasHw = 0 + + #BB visiond last type + self.last_visiond = self.cstm_btns.btns[3].btn_label2 + + + # vEgo kalman filter + dt = 0.01 + # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) + # R = 1e3 + self.v_ego_kf = KF1D(x0=np.matrix([[0.0], [0.0]]), + A=np.matrix([[1.0, dt], [0.0, 1.0]]), + C=np.matrix([1.0, 0.0]), + 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 config_ui_buttons(self, pedalPresent): + if pedalPresent: + self.btns_init[1] = [PCCModes.BUTTON_NAME, PCCModes.BUTTON_ABREVIATION, PCCModes.labels()] + else: + # we don't have pedal interceptor + self.btns_init[1] = [ACCMode.BUTTON_NAME, ACCMode.BUTTON_ABREVIATION, ACCMode.labels()] + btn = self.cstm_btns.btns[1] + btn.btn_name = self.btns_init[1][0] + btn.btn_label = self.btns_init[1][1] + btn.btn_label2 = self.btns_init[1][2][0] + btn.btn_status = 1 + self.cstm_btns.update_ui_buttons(1, 1) + + def update_ui_buttons(self,id,btn_status): + # we only focus on id=3, which is for visiond + if (id == 3) and (self.cstm_btns.btns[id].btn_status > 0) and (self.last_visiond != self.cstm_btns.btns[id].btn_label2): + self.last_visiond = self.cstm_btns.btns[id].btn_label2 + # we switched between wiggly and normal + args = ["/data/openpilot/selfdrive/car/modules/ch_visiond.sh", self.cstm_btns.btns[id].btn_label2] + subprocess.Popen(args, shell = False, stdin=None, stdout=None, stderr=None, env = dict(os.environ), close_fds=True) + + + def update(self, cp, epas_cp): + + # copy can_valid + self.can_valid = cp.can_valid + + # car params + v_weight_v = [0., 1.] # don't trust smooth speed at low values to avoid premature zero snapping + v_weight_bp = [1., 6.] # smooth blending, below ~0.6m/s the smooth speed snaps to zero + + # update prevs, update must run once per loop + self.prev_cruise_buttons = self.cruise_buttons + self.prev_blinker_on = self.blinker_on + + 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.real_carConfig = cp.vl["GTW_carConfig"] + self.real_dasHw = cp.vl["GTW_carConfig"]['GTW_dasHw'] + 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 + self.seatbelt = cp.vl["SDM1"]['SDM_bcklDrivStatus'] + #self.seatbelt = cp.vl["SDM1"]['SDM_bcklDrivStatus'] and cp.vl["GTW_status"]['GTW_driverPresent'] + + # 2 = temporary 3= TBD 4 = temporary, hit a bump 5 (permanent) 6 = temporary 7 (permanent) + # 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 + + # calc best v_ego estimate, by averaging two opposite corners + self.v_wheel_fl = 0 #JCT + self.v_wheel_fr = 0 #JCT + self.v_wheel_rl = 0 #JCT + self.v_wheel_rr = 0 #JCT + self.v_wheel = 0 #JCT + self.v_weight = 0 #JCT + speed = (cp.vl["DI_torque2"]['DI_vehicleSpeed'])*CV.MPH_TO_KPH/3.6 #JCT MPH_TO_MS. Tesla is in MPH, v_ego is expected in M/S + speed = speed * 1.01 # To match car's displayed speed + self.v_ego_x = np.matrix([[speed], [0.0]]) + self.v_ego_raw = speed + 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]) + + #BB use this set for pedal work as the user_gas_xx is used in other places + self.pedal_interceptor_state = epas_cp.vl["GAS_SENSOR"]['STATE'] + self.pedal_interceptor_value = epas_cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + self.pedal_interceptor_value2 = epas_cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2'] + + 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 = -(epas_cp.vl["EPAS_sysStatus"]['EPAS_internalSAS']) #BB see if this works better than STW_ANGLHP_STAT for angle + + self.angle_steers_rate = 0 #JCT + + 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 + + #if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY): + # self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0 + # self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE'] + # self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON'] + #else: + self.park_brake = 0 # TODO + self.brake_hold = 0 # TODO + + self.main_on = 1 #cp.vl["SCM_BUTTONS"]['MAIN_ON'] + self.cruise_speed_offset = calc_cruise_offset(cp.vl["DI_state"]['DI_cruiseSet'], self.v_ego) + self.gear_shifter = parse_gear_shifter(can_gear_shifter, self.CP.carFingerprint) + + self.pedal_gas = 0. # cp.vl["DI_torque1"]['DI_pedalPos'] / 102 #BB: to make it between 0..1 + self.car_gas = self.pedal_gas + + self.steer_override = abs(epas_cp.vl["EPAS_sysStatus"]['EPAS_handsOnLevel']) > 0 + self.steer_torque_driver = 0 #JCT + + # brake switch has shown some single time step noise, so only considered when + # switch is on for at least 2 consecutive CAN samples + # 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.standstill = cp.vl["DI_torque2"]['DI_vehicleSpeed'] == 0 + self.torqueMotor = cp.vl["DI_torque1"]['DI_torqueMotor'] + self.pcm_acc_status = cp.vl["DI_state"]['DI_cruiseState'] + self.imperial_speed_units = cp.vl["DI_state"]['DI_speedUnits'] == 0 + self.regenLight = cp.vl["DI_state"]['DI_regenLight'] == 1 + + self.prev_pedal_interceptor_available = self.pedal_interceptor_available + pedal_has_value = bool(self.pedal_interceptor_value) or bool(self.pedal_interceptor_value2) + pedal_interceptor_present = self.pedal_interceptor_state in [0, 5] and pedal_has_value + # Add loggic if we just miss some CAN messages so we don't immediately disable pedal + if pedal_has_value: + self.pedal_interceptor_missed_counter = 0 + if pedal_interceptor_present: + self.pedal_interceptor_missed_counter = 0 + else: + self.pedal_interceptor_missed_counter += 1 + pedal_interceptor_present = pedal_interceptor_present and (self.pedal_interceptor_missed_counter < 10) + # Mark pedal unavailable while traditional cruise is on. + self.pedal_interceptor_available = pedal_interceptor_present and (self.forcePedalOverCC or not bool(self.pcm_acc_status)) + if self.pedal_interceptor_available != self.prev_pedal_interceptor_available: + self.config_ui_buttons(self.pedal_interceptor_available) + + 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) + + +# carstate standalone tester +if __name__ == '__main__': + import zmq + context = zmq.Context() + + class CarParams(object): + def __init__(self): + self.carFingerprint = "TESLA MODEL S" + self.enableCruise = 0 + CP = CarParams() + CS = CarState(CP) + + # while 1: + # CS.update() + # time.sleep(0.01) diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py new file mode 100644 index 00000000000000..3985e05c941c72 --- /dev/null +++ b/selfdrive/car/tesla/interface.py @@ -0,0 +1,490 @@ +#!/usr/bin/env python +import numpy as np +from common.kalman.simple_kalman import KF1D +from cereal import car, log +from common.numpy_fast import clip, interp +from common.realtime import sec_since_boot +from selfdrive.config import Conversions as CV +from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events +from selfdrive.controls.lib.vehicle_model import VehicleModel +from selfdrive.car.tesla.carstate import CarState, get_can_parser, get_epas_parser +from selfdrive.car.tesla.values import CruiseButtons, CM, BP, AH, CAR +from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING + +try: + from selfdrive.car.tesla.carcontroller import CarController +except ImportError: + CarController = None + +AudibleAlert = car.CarControl.HUDControl.AudibleAlert +VisualAlert = car.CarControl.HUDControl.VisualAlert + + +def tesla_compute_gb(accel, speed): + return float(accel) / 3. + + +class CarInterface(object): + def __init__(self, CP, sendcan=None): + self.CP = CP + + self.frame = 0 + self.last_enable_pressed = 0 + self.last_enable_sent = 0 + self.gas_pressed_prev = False + self.brake_pressed_prev = False + self.can_invalid_count = 0 + + self.cp = get_can_parser(CP) + self.epas_cp = get_epas_parser(CP) + + # *** init the major players *** + self.CS = CarState(CP) + self.VM = VehicleModel(CP) + + # sending if read only is False + if sendcan is not None: + self.sendcan = sendcan + self.CC = CarController(self.cp.dbc_name, CP.enableCamera) + + self.compute_gb = tesla_compute_gb + + + + @staticmethod + def calc_accel_override(a_ego, a_target, v_ego, v_target): + # limit the pcm accel cmd if: + # - v_ego exceeds v_target, or + # - a_ego exceeds a_target and v_ego is close to v_target + + eA = a_ego - a_target + valuesA = [1.0, 0.1] + bpA = [0.3, 1.1] + + eV = v_ego - v_target + valuesV = [1.0, 0.1] + bpV = [0.0, 0.5] + + valuesRangeV = [1., 0.] + bpRangeV = [-1., 0.] + + # only limit if v_ego is close to v_target + speedLimiter = interp(eV, bpV, valuesV) + accelLimiter = max(interp(eA, bpA, valuesA), interp(eV, bpRangeV, valuesRangeV)) + + # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant + # unless aTargetMax is very high and then we scale with it; this help in quicker restart + + return float(max(0.714, a_target / max(_A_CRUISE_MAX_V_FOLLOWING))) * min(speedLimiter, accelLimiter) + + @staticmethod + def get_params(candidate, fingerprint): + + # kg of standard extra cargo to count for drive, gas, etc... + std_cargo = 136 + + # Scaled tire stiffness + #ts_factor = 0.8 + tire_stiffness_factor = 8 + + ret = car.CarParams.new_message() + + ret.carName = "tesla" + ret.carFingerprint = candidate + + ret.safetyModel = car.CarParams.SafetyModels.tesla + + ret.enableCamera = True + ret.enableGasInterceptor = False #keep this False for now + print "ECU Camera Simulated: ", ret.enableCamera + print "ECU Gas Interceptor: ", ret.enableGasInterceptor + + ret.enableCruise = not ret.enableGasInterceptor + + # FIXME: hardcoding honda civic 2016 touring params so they can be used to + # scale unknown params for other cars + mass = 2923 * CV.LB_TO_KG + std_cargo + wheelbase = 2.75 + centerToFront = wheelbase * 0.48 + centerToRear = wheelbase - centerToFront + rotationalInertia = 2500 + tireStiffnessFront = 95400 + tireStiffnessRear = 100000 + + #mass_models = 4722./2.205 + std_cargo + #wheelbase_models = 2.959 + # RC: I'm assuming center means center of mass, and I think Model S is pretty even between two axles + #centerToFront_models = wheelbase_models * 0.48 + #centerToRear_models = wheelbase_models - centerToFront_models + #rotationalInertia_models = 2500 + #tireStiffnessFront_models = 85400 + #tireStiffnessRear_models = 90000 + + # will create Kp and Ki for 0, 20, 40, 60 mph + ret.steerKiBP, ret.steerKpBP = [[0., 8.94, 17.88, 26.82 ], [0., 8.94, 17.88, 26.82]] + if candidate == CAR.MODELS: + stop_and_go = True + ret.mass = 4722./2.205 + std_cargo + ret.wheelbase = 2.859 + ret.centerToFront = ret.wheelbase * 0.48 + ret.steerRatio = 15 + # Kp and Ki for the lateral control for 0, 20, 40, 60 mph + ret.steerKpV, ret.steerKiV = [[1.20, 0.80, 0.60, 0.4], [0.16, 0.12, 0.08, 0.04]] + ret.steerKf = 0.00006 # Initial test value TODO: investigate FF steer control for Model S? + ret.steerActuatorDelay = 0.09 + + # Kp and Ki for the longitudinal control + ret.longitudinalKpBP = [0., 5., 35.] + ret.longitudinalKpV = [0.6, 0.6, 0.6] + ret.longitudinalKiBP = [0., 5., 35.] + ret.longitudinalKiV = [0.18,0.18,0.18] + + #from honda + #ret.longitudinalKpBP = [0., 5., 35.] + #ret.longitudinalKpV = [1.2, 0.8, 0.5] + #ret.longitudinalKiBP = [0., 35.] + #ret.longitudinalKiV = [0.18, 0.12] + # from toyota + #ret.longitudinalKpBP = [0., 5., 35.] + #ret.longitudinalKpV = [3.6, 2.4, 1.5] + #ret.longitudinalKiBP = [0., 35.] + #ret.longitudinalKiV = [0.54, 0.36] + else: + raise ValueError("unsupported car %s" % candidate) + + ret.steerControlType = car.CarParams.SteerControlType.angle + + # min speed to enable ACC. if car can do stop and go, then set enabling speed + # to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not + # conflict with PCM acc + ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 25.5 * CV.MPH_TO_MS + + centerToRear = ret.wheelbase - ret.centerToFront + # TODO: get actual value, for now starting with reasonable value for + # civic and scaling by mass and wheelbase + ret.rotationalInertia = rotationalInertia * \ + ret.mass * ret.wheelbase**2 / (mass * wheelbase**2) + + # TODO: start from empirically derived lateral slip stiffness for the civic and scale by + # mass and CG position, so all cars will have approximately similar dyn behaviors + ret.tireStiffnessFront = (tireStiffnessFront * tire_stiffness_factor) * \ + ret.mass / mass * \ + (centerToRear / ret.wheelbase) / (centerToRear / wheelbase) + ret.tireStiffnessRear = (tireStiffnessRear * tire_stiffness_factor) * \ + ret.mass / mass * \ + (ret.centerToFront / ret.wheelbase) / (centerToFront / wheelbase) + + # no rear steering, at least on the listed cars above + ret.steerRatioRear = 0. + + # no max steer limit VS speed + ret.steerMaxBP = [0.,15.] # m/s + ret.steerMaxV = [420.,420.] # max steer allowed + + ret.gasMaxBP = [0.] # m/s + ret.gasMaxV = [0.3] #if ret.enableGasInterceptor else [0.] # max gas allowed + ret.brakeMaxBP = [0., 20.] # m/s + ret.brakeMaxV = [1., 1.] # max brake allowed - BB: since we are using regen, make this even + + ret.longPidDeadzoneBP = [0., 9.] #BB: added from Toyota to start pedal work; need to tune + ret.longPidDeadzoneV = [0., 0.] #BB: added from Toyota to start pedal work; need to tune; changed to 0 for now + + ret.stoppingControl = True + ret.openpilotLongitudinalControl = True + ret.steerLimitAlert = False + ret.startAccel = 0.5 + ret.steerRateCost = 1.5 + + return ret + + # returns a car.CarState + def update(self, c): + # ******************* do can recv ******************* + canMonoTimes = [] + + self.cp.update(int(sec_since_boot() * 1e9), False) + self.epas_cp.update(int(sec_since_boot() * 1e9), False) + + self.CS.update(self.cp, self.epas_cp) + + # create message + ret = car.CarState.new_message() + + # speeds + ret.vEgo = self.CS.v_ego + ret.aEgo = self.CS.a_ego + ret.vEgoRaw = self.CS.v_ego_raw + ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) + ret.standstill = self.CS.standstill + ret.wheelSpeeds.fl = self.CS.v_wheel_fl + ret.wheelSpeeds.fr = self.CS.v_wheel_fr + ret.wheelSpeeds.rl = self.CS.v_wheel_rl + ret.wheelSpeeds.rr = self.CS.v_wheel_rr + + # gas pedal, we don't use with with interceptor so it's always 0/False + ret.gas = self.CS.user_gas + if not self.CP.enableGasInterceptor: + ret.gasPressed = self.CS.user_gas_pressed + else: + ret.gasPressed = self.CS.user_gas_pressed + + + + # brake pedal + ret.brakePressed =False # (self.CS.brake_pressed != 0) and (self.CS.cstm_btns.get_button_status("brake") == 0) + # FIXME: read sendcan for brakelights + brakelights_threshold = 0.1 + ret.brakeLights = bool(self.CS.brake_switch or + c.actuators.brake > brakelights_threshold) + + # steering wheel + ret.steeringAngle = self.CS.angle_steers + ret.steeringRate = self.CS.angle_steers_rate + + # gear shifter lever + ret.gearShifter = self.CS.gear_shifter + + ret.steeringTorque = self.CS.steer_torque_driver + ret.steeringPressed = self.CS.steer_override + + # cruise state + ret.cruiseState.enabled = True #self.CS.pcm_acc_status != 0 + ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS + ret.cruiseState.available = bool(self.CS.main_on) + ret.cruiseState.speedOffset = self.CS.cruise_speed_offset + ret.cruiseState.standstill = False + + # TODO: button presses + buttonEvents = [] + ret.leftBlinker = bool(self.CS.left_blinker_on) + ret.rightBlinker = bool(self.CS.right_blinker_on) + + + ret.doorOpen = not self.CS.door_all_closed + ret.seatbeltUnlatched = not self.CS.seatbelt + + if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: + be = car.CarState.ButtonEvent.new_message() + be.type = 'leftBlinker' + be.pressed = self.CS.left_blinker_on != 0 + buttonEvents.append(be) + + if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: + be = car.CarState.ButtonEvent.new_message() + be.type = 'rightBlinker' + be.pressed = self.CS.right_blinker_on != 0 + buttonEvents.append(be) + + if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: + be = car.CarState.ButtonEvent.new_message() + be.type = 'unknown' + if self.CS.cruise_buttons != 0: + be.pressed = True + but = self.CS.cruise_buttons + else: + be.pressed = False + but = self.CS.prev_cruise_buttons + if but == CruiseButtons.RES_ACCEL: + be.type = 'accelCruise' + elif but == CruiseButtons.DECEL_SET: + be.type = 'decelCruise' + elif but == CruiseButtons.CANCEL: + be.type = 'cancel' + elif but == CruiseButtons.MAIN: + be.type = 'altButton3' + buttonEvents.append(be) + + if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: + be = car.CarState.ButtonEvent.new_message() + be.type = 'unknown' + be.pressed = bool(self.CS.cruise_buttons) + buttonEvents.append(be) + ret.buttonEvents = buttonEvents + + # events + # TODO: I don't like the way capnp does enums + # These strings aren't checked at compile time + events = [] + + #notification messages for DAS + self.CS.DAS_noSeatbelt = 0 + self.CS.DAS_canErrors = 0 + self.CS.DAS_plannerErrors = 0 + self.CS.DAS_doorOpen = 0 + self.CS.DAS_notInDrive = 0 + self.CC.opState = 0 + if c.enabled: + self.CC.opState = 1 + if not self.CS.can_valid: + self.can_invalid_count += 1 + if self.can_invalid_count >= 25: #BB increased to 25 to see if we still get the can error messages + events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + self.CS.DAS_canErrors = 1 + if self.CC.opState > 0: + self.CC.opState = 2 + else: + self.can_invalid_count = 0 + if self.CS.steer_error: + if not self.CS.enableHSO: + events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.WARNING])) + elif self.CS.steer_warning: + if not self.CS.enableHSO: + events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + if self.CC.opState > 0: + self.CC.opState = 2 + if self.CS.brake_error: + events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) + if self.CC.opState > 0: + self.CC.opState = 2 + if not ret.gearShifter == 'drive': + events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + self.CS.DAS_notInDrive = 1 + if self.CC.opState > 0: + self.CC.opState = 0 + if ret.doorOpen: + events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + self.CS.DAS_doorOpen = 1 + if self.CC.opState > 0: + self.CC.opState = 0 + if ret.seatbeltUnlatched: + events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if c.enabled: + self.CS.DAS_noSeatbelt = 1 + if self.CC.opState > 0: + self.CC.opState = 2 + if self.CS.esp_disabled: + events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if self.CC.opState > 0: + self.CC.opState = 2 + if not self.CS.main_on: + events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) + if self.CC.opState > 0: + self.CC.opState = 0 + if ret.gearShifter == 'reverse': + events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + self.CS.DAS_notInDrive = 1 + if self.CC.opState > 0: + self.CC.opState = 0 + if self.CS.brake_hold: + events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE])) + if self.CC.opState > 0: + self.CC.opState = 0 + if self.CS.park_brake: + events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) + if self.CC.opState > 0: + self.CC.opState = 0 + + + if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed: + events.append(create_event('speedTooLow', [ET.NO_ENTRY])) + +# Standard OP method to disengage: +# disable on pedals rising edge or when brake is pressed and speed isn't zero +# if (ret.gasPressed and not self.gas_pressed_prev) or \ +# (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): +# events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + + #if (self.CS.cstm_btns.get_button_status("brake")>0): + # if ((self.CS.brake_pressed !=0) != self.brake_pressed_prev): #break not canceling when pressed + # self.CS.cstm_btns.set_button_status("brake", 2 if self.CS.brake_pressed != 0 else 1) + #else: + # if ret.brakePressed: + # events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) + if ret.gasPressed: + events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) + + # it can happen that car cruise disables while comma system is enabled: need to + # keep braking if needed or if the speed is very low + if self.CP.enableCruise and not ret.cruiseState.enabled and c.actuators.brake <= 0.: + # non loud alert if cruise disbales below 25mph as expected (+ a little margin) + if ret.vEgo < self.CP.minEnableSpeed + 2.: + events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) + else: + events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) + if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001: + events.append(create_event('manualRestart', [ET.WARNING])) + + cur_time = sec_since_boot() + enable_pressed = False + # handle button presses + for b in ret.buttonEvents: + + # do enable on both accel and decel buttons + if b.type == "altButton3" and not b.pressed: + print "enabled pressed at", cur_time + self.last_enable_pressed = cur_time + enable_pressed = True + + # do disable on button down + if b.type == "cancel" and b.pressed: + events.append(create_event('buttonCancel', [ET.USER_DISABLE])) + + if self.CP.enableCruise: + # KEEP THIS EVENT LAST! send enable event if button is pressed and there are + # NO_ENTRY events, so controlsd will display alerts. Also not send enable events + # too close in time, so a no_entry will not be followed by another one. + # TODO: button press should be the only thing that triggers enble + if ((cur_time - self.last_enable_pressed) < 0.2 and + (cur_time - self.last_enable_sent) > 0.2 and + ret.cruiseState.enabled) or \ + (enable_pressed and get_events(events, [ET.NO_ENTRY])): + events.append(create_event('buttonEnable', [ET.ENABLE])) + self.last_enable_sent = cur_time + elif enable_pressed: + events.append(create_event('buttonEnable', [ET.ENABLE])) + + ret.events = events + ret.canMonoTimes = canMonoTimes + + # update previous brake/gas pressed + self.gas_pressed_prev = ret.gasPressed + self.brake_pressed_prev = self.CS.brake_pressed != 0 + + # cast to reader so it can't be modified + return ret.as_reader() + + # pass in a car.CarControl + # to be called @ 100hz + def apply(self, c, perception_state=log.Live20Data.new_message()): + if c.hudControl.speedVisible: + hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH + else: + hud_v_cruise = 255 + + VISUAL_HUD = { + VisualAlert.none: AH.NONE, + VisualAlert.fcw: AH.FCW, + VisualAlert.steerRequired: AH.STEER, + VisualAlert.brakePressed: AH.BRAKE_PRESSED, + VisualAlert.wrongGear: AH.GEAR_NOT_D, + VisualAlert.seatbeltUnbuckled: AH.SEATBELT, + VisualAlert.speedTooHigh: AH.SPEED_TOO_HIGH} + + AUDIO_HUD = { + AudibleAlert.none: (BP.MUTE, CM.MUTE), + AudibleAlert.chimeEngage: (BP.SINGLE, CM.MUTE), + AudibleAlert.chimeDisengage: (BP.SINGLE, CM.MUTE), + AudibleAlert.chimeError: (BP.MUTE, CM.DOUBLE), + AudibleAlert.chimePrompt: (BP.MUTE, CM.SINGLE), + AudibleAlert.chimeWarning1: (BP.MUTE, CM.DOUBLE), + AudibleAlert.chimeWarning2: (BP.MUTE, CM.REPEATED), + AudibleAlert.chimeWarningRepeat: (BP.MUTE, CM.REPEATED)} + + hud_alert = VISUAL_HUD[c.hudControl.visualAlert.raw] + snd_beep, snd_chime = AUDIO_HUD[c.hudControl.audibleAlert.raw] + + pcm_accel = int(clip(c.cruiseControl.accelOverride,0,1)*0xc6) + + self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, \ + c.actuators, \ + c.cruiseControl.speedOverride, \ + c.cruiseControl.override, \ + c.cruiseControl.cancel, \ + pcm_accel, \ + hud_v_cruise, c.hudControl.lanesVisible, \ + hud_show_car = c.hudControl.leadVisible, \ + hud_alert = hud_alert, \ + snd_beep = snd_beep, \ + snd_chime = snd_chime) + + self.frame += 1 diff --git a/selfdrive/car/tesla/longcontrol_tesla.py b/selfdrive/car/tesla/longcontrol_tesla.py new file mode 100644 index 00000000000000..633269670983bf --- /dev/null +++ b/selfdrive/car/tesla/longcontrol_tesla.py @@ -0,0 +1,131 @@ +from common.numpy_fast import clip, interp +from selfdrive.controls.lib.pid import PIController + +STOPPING_EGO_SPEED = 0.5 +MIN_CAN_SPEED = 0.3 #TODO: parametrize this in car interface +STOPPING_TARGET_SPEED = MIN_CAN_SPEED + 0.01 +STARTING_TARGET_SPEED = 0.5 +BRAKE_THRESHOLD_TO_PID = 0.2 + + +class LongCtrlState: + #*** this function handles the long control state transitions + # long_control_state labels (see capnp enum): + off = 'off' # Off + pid = 'pid' # moving and tracking targets, with PID control running + stopping = 'stopping' # stopping and changing controls to almost open loop as PID does not fit well at such a low speed + starting = 'starting' # starting and releasing brakes in open loop before giving back to PID + + + +def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, + output_gb, brake_pressed, cruise_standstill): + + stopping_condition = (v_ego < 2.0 and cruise_standstill) or \ + (v_ego < STOPPING_EGO_SPEED and \ + ((v_pid < STOPPING_TARGET_SPEED and v_target < STOPPING_TARGET_SPEED) or + brake_pressed)) + + starting_condition = v_target > STARTING_TARGET_SPEED and not cruise_standstill + + if not active: + long_control_state = LongCtrlState.off + + else: + if long_control_state == LongCtrlState.off: + if active: + long_control_state = LongCtrlState.pid + + elif long_control_state == LongCtrlState.pid: + if stopping_condition: + long_control_state = LongCtrlState.stopping + + elif long_control_state == LongCtrlState.stopping: + if starting_condition: + long_control_state = LongCtrlState.starting + + elif long_control_state == LongCtrlState.starting: + if stopping_condition: + long_control_state = LongCtrlState.stopping + elif output_gb >= -BRAKE_THRESHOLD_TO_PID: + long_control_state = LongCtrlState.pid + + return long_control_state + + +stopping_brake_rate = 0.2 # brake_travel/s while trying to stop +starting_brake_rate = 0.8 # brake_travel/s while releasing on restart +brake_stopping_target = 0.5 # apply at least this amount of brake to maintain the vehicle stationary + +_MAX_SPEED_ERROR_BP = [0., 30.] # speed breakpoints +_MAX_SPEED_ERROR_V = [1.5, .8] # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp + + +class LongControl(object): + def __init__(self, CP, compute_gb): + self.long_control_state = LongCtrlState.off # initialized to off + self.pid = PIController((CP.longitudinalKpBP, CP.longitudinalKpV), + (CP.longitudinalKiBP, CP.longitudinalKiV), + rate=100.0, + sat_limit=0.8, + convert=compute_gb) + self.v_pid = 0.0 + self.last_output_gb = 0.0 + + def reset(self, v_pid): + self.pid.reset() + self.v_pid = v_pid + + def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, lead_1): + # actuation limits + gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV) + brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV) + + output_gb = self.last_output_gb + rate = 100.0 + self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego, + v_target_future, self.v_pid, output_gb, + brake_pressed, cruise_standstill) + + v_ego_pid = max(v_ego, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 + + # *** long control behavior based on state + if self.long_control_state == LongCtrlState.off: + self.v_pid = v_ego_pid # do nothing + output_gb = 0. + self.pid.reset() + + # tracking objects and driving + elif self.long_control_state == LongCtrlState.pid: + prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7 + + self.v_pid = v_target + self.pid.pos_limit = gas_max + self.pid.neg_limit = - brake_max + deadzone = interp(v_ego_pid, CP.longPidDeadzoneBP, CP.longPidDeadzoneV) + output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot) + if prevent_overshoot: + output_gb = min(output_gb, 0.0) + + # intention is to stop, switch to a different brake control until we stop + elif self.long_control_state == LongCtrlState.stopping: + # TODO: use the standstill bit from CAN to detect movements, usually more accurate than looking at v_ego + if not standstill or output_gb > -brake_stopping_target: + output_gb -= stopping_brake_rate / rate + output_gb = clip(output_gb, -brake_max, gas_max) + + self.v_pid = v_ego + self.pid.reset() + + # intention is to move again, release brake fast before handling control to PID + elif self.long_control_state == LongCtrlState.starting: + if output_gb < -0.2: + output_gb += starting_brake_rate / rate + self.v_pid = v_ego + self.pid.reset() + + self.last_output_gb = output_gb + final_gas = clip(output_gb, 0., gas_max) + final_brake = -clip(output_gb, -brake_max, 0.) + + return final_gas, final_brake \ No newline at end of file diff --git a/selfdrive/car/tesla/old_can_parser.py b/selfdrive/car/tesla/old_can_parser.py new file mode 100644 index 00000000000000..da47ecd7030eb9 --- /dev/null +++ b/selfdrive/car/tesla/old_can_parser.py @@ -0,0 +1,130 @@ +import os +import opendbc +from collections import defaultdict + +from selfdrive.car.honda.hondacan import fix +from common.realtime import sec_since_boot +from common.dbc import dbc + +class CANParser(object): + def __init__(self, dbc_f, signals, checks=None): + ### input: + # dbc_f : dbc file + # signals : List of tuples (name, address, ival) where + # - name is the signal name. + # - address is the corresponding message address. + # - ival is the initial value. + # checks : List of pairs (address, frequency) where + # - address is the message address of a message for which health should be + # monitored. + # - frequency is the frequency at which health should be monitored. + + checks = [] if checks is None else checks + self.msgs_ck = set([check[0] for check in checks]) + self.frqs = dict(checks) + self.can_valid = False # start with False CAN assumption + # list of received msg we want to monitor counter and checksum for + # read dbc file + self.can_dbc = dbc(os.path.join(opendbc.DBC_PATH, dbc_f)) + # initialize variables to initial values + self.vl = {} # signal values + self.ts = {} # time stamp recorded in log + self.ct = {} # current time stamp + self.ok = {} # valid message? + self.cn = {} # message counter + self.cn_vl = {} # message counter mismatch value + self.ck = {} # message checksum status + + for _, addr, _ in signals: + self.vl[addr] = {} + self.ts[addr] = {} + self.ct[addr] = sec_since_boot() + self.ok[addr] = True + self.cn[addr] = 0 + self.cn_vl[addr] = 0 + self.ck[addr] = False + + for name, addr, ival in signals: + self.vl[addr][name] = ival + self.ts[addr][name] = 0 + + self._msgs = [s[1] for s in signals] + self._sgs = [s[0] for s in signals] + + self._message_indices = defaultdict(list) + for i, x in enumerate(self._msgs): + self._message_indices[x].append(i) + + def update_can(self, can_recv): + msgs_upd = [] + cn_vl_max = 5 # no more than 5 wrong counter checks + + self.sec_since_boot_cached = sec_since_boot() + + # we are subscribing to PID_XXX, else data from USB + for msg, ts, cdat, _ in can_recv: + idxs = self._message_indices[msg] + if idxs: + msgs_upd.append(msg) + # read the entire message + out = self.can_dbc.decode((msg, 0, cdat))[1] + # checksum check + self.ck[msg] = True + if "CHECKSUM" in out.keys() and msg in self.msgs_ck: + # remove checksum (half byte) + ck_portion = cdat[:-1] + chr(ord(cdat[-1]) & 0xF0) + # recalculate checksum + msg_vl = fix(ck_portion, msg) + # compare recalculated vs received checksum + if msg_vl != cdat: + print "CHECKSUM FAIL: " + hex(msg) + self.ck[msg] = False + self.ok[msg] = False + # counter check + cn = 0 + if "COUNTER" in out.keys(): + cn = out["COUNTER"] + # check counter validity if it's a relevant message + if cn != ((self.cn[msg] + 1) % 4) and msg in self.msgs_ck and "COUNTER" in out.keys(): + #print hex(msg), "FAILED COUNTER!" + self.cn_vl[msg] += 1 # counter check failed + else: + self.cn_vl[msg] -= 1 # counter check passed + # message status is invalid if we received too many wrong counter values + if self.cn_vl[msg] >= cn_vl_max: + print "COUNTER WRONG: " + hex(msg) + self.ok[msg] = False + + # update msg time stamps and counter value + self.ct[msg] = self.sec_since_boot_cached + self.cn[msg] = cn + self.cn_vl[msg] = min(max(self.cn_vl[msg], 0), cn_vl_max) + + # set msg valid status if checksum is good and wrong counter counter is zero + if self.ck[msg] and self.cn_vl[msg] == 0: + self.ok[msg] = True + + # update value of signals in the + for ii in idxs: + sg = self._sgs[ii] + self.vl[msg][sg] = out[sg] + self.ts[msg][sg] = ts + + # for each message, check if it's too long since last time we received it + self._check_dead_msgs() + + # assess overall can validity: if there is one relevant message invalid, then set can validity flag to False + self.can_valid = True + + if False in self.ok.values(): + #print "CAN INVALID!" + self.can_valid = False + + return msgs_upd + + def _check_dead_msgs(self): + ### input: + ## simple stuff for now: msg is not valid if a message isn't received for 10 consecutive steps + for msg in set(self._msgs): + if msg in self.msgs_ck and self.sec_since_boot_cached - self.ct[msg] > 10./self.frqs[msg]: + self.ok[msg] = False diff --git a/selfdrive/car/tesla/radar_interface.py b/selfdrive/car/tesla/radar_interface.py new file mode 100644 index 00000000000000..49691885972a52 --- /dev/null +++ b/selfdrive/car/tesla/radar_interface.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python +from cereal import car +import time + + +class RadarInterface(object): + def __init__(self, CP): + # radar + self.pts = {} + self.delay = 0.1 + + def update(self): + + ret = car.RadarState.new_message() + time.sleep(0.05) # radard runs on RI updates + + return ret + +if __name__ == "__main__": + RI = RadarInterface() + while 1: + ret = RI.update() + print(chr(27) + "[2J") + print(ret) diff --git a/selfdrive/car/tesla/readconfig.py b/selfdrive/car/tesla/readconfig.py new file mode 100644 index 00000000000000..ef9789e1d629da --- /dev/null +++ b/selfdrive/car/tesla/readconfig.py @@ -0,0 +1,57 @@ +import ConfigParser + +config_path = '/data/bb_openpilot.cfg' +config_file_r = 'r' +config_file_w = 'wb' + +def read_config_file(CS): + configr = ConfigParser.ConfigParser() + configr.read(config_path) + config = ConfigParser.RawConfigParser() + config.add_section('OP_CONFIG') + + #force_pedal_over_cc -> CS.forcePedalOverCC + try: + CS.forcePedalOverCC = configr.getboolean('OP_CONFIG','force_pedal_over_cc') + except: + CS.forcePedalOverCC = True + config.set('OP_CONFIG', 'force_pedal_over_cc', CS.forcePedalOverCC) + + #enable_hso -> CS.enableHSO + try: + CS.enableHSO = configr.getboolean('OP_CONFIG','enable_hso') + except: + CS.enableHSO = True + config.set('OP_CONFIG', 'enable_hso', CS.enableHSO) + + #enable_alca -> CS.enableALCA + try: + CS.enableALCA = configr.getboolean('OP_CONFIG','enable_alca') + except: + CS.enableALCA = True + config.set('OP_CONFIG', 'enable_alca', CS.enableALCA) + + #enable_das_emulation -> CS.enableDasEmulation + try: + CS.enableDasEmulation = configr.getboolean('OP_CONFIG','enable_das_emulation') + except: + CS.enableDasEmulation = True + config.set('OP_CONFIG', 'enable_das_emulation', CS.enableDasEmulation) + + #enable_radar_emulation -> CS.enableRadarEmulation + try: + CS.enableRadarEmulation = configr.getboolean('OP_CONFIG','enable_radar_emulation') + except: + CS.enableRadarEmulation = True + config.set('OP_CONFIG', 'enable_radar_emulation', CS.enableRadarEmulation) + + #enable_speed_variable_angle -> CS.enableSpeedVariableDesAngle + try: + CS.enableSpeedVariableDesAngle = configr.getboolean('OP_CONFIG','enable_speed_variable_angle') + except: + CS.enableSpeedVariableDesAngle = True + config.set('OP_CONFIG', 'enable_speed_variable_angle', CS.enableSpeedVariableDesAngle) + + with open(config_path, config_file_w) as configfile: + config.write(configfile) + diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py new file mode 100644 index 00000000000000..c194c6558bf5d1 --- /dev/null +++ b/selfdrive/car/tesla/teslacan.py @@ -0,0 +1,159 @@ +import struct +from ctypes import create_string_buffer + +def add_tesla_crc(msg,msg_len): + """Calculate CRC8 using 1D poly, FF start, FF end""" + crc_lookup = [0x00, 0x1D, 0x3A, 0x27, 0x74, 0x69, 0x4E, 0x53, 0xE8, 0xF5, 0xD2, 0xCF, 0x9C, 0x81, 0xA6, 0xBB, +0xCD, 0xD0, 0xF7, 0xEA, 0xB9, 0xA4, 0x83, 0x9E, 0x25, 0x38, 0x1F, 0x02, 0x51, 0x4C, 0x6B, 0x76, +0x87, 0x9A, 0xBD, 0xA0, 0xF3, 0xEE, 0xC9, 0xD4, 0x6F, 0x72, 0x55, 0x48, 0x1B, 0x06, 0x21, 0x3C, +0x4A, 0x57, 0x70, 0x6D, 0x3E, 0x23, 0x04, 0x19, 0xA2, 0xBF, 0x98, 0x85, 0xD6, 0xCB, 0xEC, 0xF1, +0x13, 0x0E, 0x29, 0x34, 0x67, 0x7A, 0x5D, 0x40, 0xFB, 0xE6, 0xC1, 0xDC, 0x8F, 0x92, 0xB5, 0xA8, +0xDE, 0xC3, 0xE4, 0xF9, 0xAA, 0xB7, 0x90, 0x8D, 0x36, 0x2B, 0x0C, 0x11, 0x42, 0x5F, 0x78, 0x65, +0x94, 0x89, 0xAE, 0xB3, 0xE0, 0xFD, 0xDA, 0xC7, 0x7C, 0x61, 0x46, 0x5B, 0x08, 0x15, 0x32, 0x2F, +0x59, 0x44, 0x63, 0x7E, 0x2D, 0x30, 0x17, 0x0A, 0xB1, 0xAC, 0x8B, 0x96, 0xC5, 0xD8, 0xFF, 0xE2, +0x26, 0x3B, 0x1C, 0x01, 0x52, 0x4F, 0x68, 0x75, 0xCE, 0xD3, 0xF4, 0xE9, 0xBA, 0xA7, 0x80, 0x9D, +0xEB, 0xF6, 0xD1, 0xCC, 0x9F, 0x82, 0xA5, 0xB8, 0x03, 0x1E, 0x39, 0x24, 0x77, 0x6A, 0x4D, 0x50, +0xA1, 0xBC, 0x9B, 0x86, 0xD5, 0xC8, 0xEF, 0xF2, 0x49, 0x54, 0x73, 0x6E, 0x3D, 0x20, 0x07, 0x1A, +0x6C, 0x71, 0x56, 0x4B, 0x18, 0x05, 0x22, 0x3F, 0x84, 0x99, 0xBE, 0xA3, 0xF0, 0xED, 0xCA, 0xD7, +0x35, 0x28, 0x0F, 0x12, 0x41, 0x5C, 0x7B, 0x66, 0xDD, 0xC0, 0xE7, 0xFA, 0xA9, 0xB4, 0x93, 0x8E, +0xF8, 0xE5, 0xC2, 0xDF, 0x8C, 0x91, 0xB6, 0xAB, 0x10, 0x0D, 0x2A, 0x37, 0x64, 0x79, 0x5E, 0x43, +0xB2, 0xAF, 0x88, 0x95, 0xC6, 0xDB, 0xFC, 0xE1, 0x5A, 0x47, 0x60, 0x7D, 0x2E, 0x33, 0x14, 0x09, +0x7F, 0x62, 0x45, 0x58, 0x0B, 0x16, 0x31, 0x2C, 0x97, 0x8A, 0xAD, 0xB0, 0xE3, 0xFE, 0xD9, 0xC4] + crc = 0xFF + for x in range(0,msg_len,1): + crc = crc_lookup[crc ^ ord(msg[x])] + crc = crc ^ 0xFF + return crc + + +def add_tesla_checksum(msg_id,msg): + """Calculates the checksum for the data part of the Tesla message""" + checksum = ((msg_id) & 0xFF) + ((msg_id >> 8) & 0xFF) + for i in range(0,len(msg),1): + checksum = (checksum + ord(msg[i])) & 0xFF + return checksum + + +def create_pedal_command_msg(accelCommand, enable, idx): + """Create GAS_COMMAND (0x551) message to comma pedal""" + msg_id = 0x551 + msg_len = 6 + msg = create_string_buffer(msg_len) + m1 = 0.050796813 + m2 = 0.101593626 + d = -22.85856576 + if enable == 1: + int_accelCommand = int((accelCommand - d)/m1) + int_accelCommand2 = int((accelCommand - d)/m2) + else: + int_accelCommand = 0 + int_accelCommand2 = 0 + msg = create_string_buffer(msg_len) + struct.pack_into('BBBBB', msg, 0, (int_accelCommand >> 8) & 0xFF, int_accelCommand & 0xFF, \ + (int_accelCommand2 >> 8) & 0xFF, int_accelCommand2 & 0XFF,((enable << 7) + idx) & 0xFF) + struct.pack_into('B', msg, msg_len-1, add_tesla_checksum(msg_id,msg)) + return [msg_id, 0, msg.raw, 2] + + + +def create_fake_DAS_msg(speed_control_enabled,gas_to_resume,apUnavailable, collision_warning, op_status, \ + acc_speed_kph, \ + turn_signal_needed,forward_collission_warning,hands_on_state, \ + cc_state, pedal_state, alca_state, \ + acc_speed_limit_mph, + legal_speed_limit, + apply_angle, + enable_steer_control): + msg_id = 0x553 + msg_len = 8 + msg = create_string_buffer(msg_len) + c_apply_steer = ((int( apply_angle * 10 + 0x4000 )) & 0x7FFF) + (enable_steer_control << 15) + struct.pack_into('BBBBBBBB', msg, 0,(speed_control_enabled << 7) + (gas_to_resume << 6) + (apUnavailable << 5) + (collision_warning << 4) + op_status, \ + acc_speed_kph, \ + (turn_signal_needed << 6) + (forward_collission_warning << 4) + hands_on_state, \ + (cc_state << 6) + (pedal_state << 5) + alca_state, \ + acc_speed_limit_mph, + legal_speed_limit, + c_apply_steer & 0xFF, + (c_apply_steer >> 8) & 0xFF) + return [msg_id, 0, msg.raw, 0] + +def create_fake_DAS_warning(DAS_noSeatbelt, DAS_canErrors, DAS_plannerErrors, DAS_doorOpen, DAS_notInDrive,\ + enableFakeDas,enableRadar): + msg_id = 0x554 + msg_len = 1 + fd = 0 + rd = 0 + if enableFakeDas: + fd = 1 + if enableRadar: + rd = 1 + warn = (rd << 6) + (fd << 5) + (DAS_noSeatbelt << 4) + (DAS_canErrors << 3) + (DAS_plannerErrors << 2) + (DAS_doorOpen << 1) + DAS_notInDrive + msg = create_string_buffer(msg_len) + struct.pack_into('B',msg ,0 , warn) + 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) + None 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) + None 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) + # 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 is not None: + # 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 + if turnIndLvr_Stat is not None: + fake_stalk['TurnIndLvr_Stat'] = turnIndLvr_Stat + # message count should be 1 more than the previous (and loop after 16) + 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/teslaradar.dbc b/selfdrive/car/tesla/teslaradar.dbc new file mode 100644 index 00000000000000..b7d10239ff09bb --- /dev/null +++ b/selfdrive/car/tesla/teslaradar.dbc @@ -0,0 +1,325 @@ +VERSION "" + + +NS_ : + NS_DESC_ + CM_ + BA_DEF_ + BA_ + VAL_ + CAT_DEF_ + CAT_ + FILTER + BA_DEF_DEF_ + EV_DATA_ + ENVVAR_DATA_ + SGTYPE_ + SGTYPE_VAL_ + BA_DEF_SGTYPE_ + BA_SGTYPE_ + SIG_TYPE_REF_ + VAL_TABLE_ + SIG_GROUP_ + SIG_VALTYPE_ + SIGTYPE_VALTYPE_ + BO_TX_BU_ + BA_DEF_REL_ + BA_REL_ + BA_DEF_DEF_REL_ + BU_SG_REL_ + BU_EV_REL_ + BU_BO_REL_ + SG_MUL_VAL_ + +BS_: + +BU_: XXX + + +BO_ 784 310hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 785 310hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 787 313hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 788 313hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 790 316hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 791 316hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 793 319hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 794 319hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 796 31Chex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 797 31Chex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 799 31Fhex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 800 31Fhex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 802 322hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 803 322hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 805 325hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 806 325hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 808 328hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 809 328hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 811 32Bhex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 812 32Bhex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 814 32Ehex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 815 32Ehex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 817 331hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 818 331hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 820 334hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 821 334hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 823 337hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 824 337hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 826 33Ahex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 827 33Ahex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 829 33Dhex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 830 33Dhex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 832 340hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 833 340hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 835 343hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 836 343hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 838 346hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 839 346hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 841 349hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 842 349hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 844 34Chex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 845 34Chex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 847 34Fhex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 848 34Fhex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 850 352hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 851 352hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 853 355hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 854 355hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 856 358hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 857 358hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 859 35Bhex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 860 35Bhex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 862 35Ehex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 863 35Ehex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 865 361hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 866 361hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 868 364hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 869 364hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 871 367hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 872 367hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 874 370hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 875 370hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX + +BO_ 877 373hex: 8 XXX + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" XXX + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" XXX + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" XXX + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" XXX + +BO_ 878 373hex2: 8 XXX + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" XXX diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py new file mode 100644 index 00000000000000..7e93d56617f659 --- /dev/null +++ b/selfdrive/car/tesla/values.py @@ -0,0 +1,142 @@ +from selfdrive.car import dbc_dict + +class CAR: + MODELS = "TESLA MODEL S" + + +FINGERPRINTS = { + CAR.MODELS: [{ + 1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 267: 5, 277: 4, 280: 6, 283: 5, 293: 4, 296: 4, 309: 5, 336: 8, 341: 8, 357: 8, 360: 7, 415: 8, 513: 5, 516: 8, 520: 4, 522: 8, 524: 8, 526: 8, 527: 8, 536: 8, 542: 8, 551: 4, 552: 2, 556: 8, 558: 8, 568: 8, 574: 8, 582: 5, 590: 8, 606: 8, 622: 8, 638: 8, 643: 8, 693: 8, 696: 8, 712: 8, 728: 8, 744: 8, 760: 8, 771: 2, 772: 8, 775: 8, 776: 8, 778: 8, 780: 2, 782: 8, 783: 8, 785: 8, 787: 8, 788: 8, 791: 8, 792: 8, 796: 2, 798: 6, 799: 8, 804: 8, 807: 8, 808: 1, 812: 8, 814: 5, 815: 8, 820: 8, 823: 8, 824: 8, 830: 5, 831: 8, 836: 8, 840: 8, 846: 5, 856: 4, 862: 5, 863: 8, 872: 8, 878: 8, 879: 8, 880: 8, 888: 8, 896: 8, 904: 3, 920: 8, 936: 8, 952: 8, 953: 6, 984: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1034: 8, 1048: 1, 1281: 8, 1332: 8, 1335: 8, 1361: 6, 1362: 6, 1368: 8, 1412: 8, 1436: 8, 1456: 8, 1486: 8, 1519: 8, 1524: 8, 1527: 8, 1601: 8, 1605: 8, 1611: 8, 1614: 8, 1617: 8, 1621: 8, 1627: 8, 1630: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1824: 1, 1828: 8, 1831: 8, 1832: 8, 1840: 8, 1848: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2043: 5 + }], +} + + +DBC = { + CAR.MODELS: dbc_dict('tesla_can', None), +} + + +# 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 + RES_ACCEL_2ND = 4 + DECEL_SET = 32 + DECEL_2ND = 8 + CANCEL = 1 + MAIN = 2 + IDLE = 0 + + @classmethod + def is_accel(cls, btn): + return btn in [cls.RES_ACCEL, cls.RES_ACCEL_2ND] + + @classmethod + def is_decel(cls, btn): + return btn in [cls.DECEL_SET, cls.DECEL_2ND] + + @classmethod + def should_be_throttled(cls, btn): + # Some buttons should not be spammed or they may overwhelm the SCCM. + return btn not in [cls.MAIN, cls.IDLE] + + +#car chimes: enumeration from dbc file. Chimes are for alerts and warnings +class CM: + MUTE = 0 + SINGLE = 3 + DOUBLE = 4 + REPEATED = 1 + CONTINUOUS = 2 + + +#car beepss: enumeration from dbc file. Beeps are for activ and deactiv +class BP: + MUTE = 0 + SINGLE = 3 + TRIPLE = 2 + REPEATED = 1 + +class AH: + #[alert_idx, value] + # See dbc files for info on values" + NONE = [0, 0] + FCW = [1, 1] + STEER = [2, 1] + BRAKE_PRESSED = [3, 10] + GEAR_NOT_D = [4, 6] + SEATBELT = [5, 5] + SPEED_TOO_HIGH = [6, 8] + +class CruiseState: + # DI_cruiseState from the DBC + OFF = 0 + STANDBY = 1 + ENABLED = 2 + STANDSTILL = 3 + OVERRIDE = 4 + FAULT = 5 + PRE_FAULT = 6 + PRE_CANCEL = 7 + + @classmethod + def is_enabled_or_standby(cls, state): + return state in [cls.ENABLED, cls.STANDBY] + + @classmethod + def is_faulted(cls, state): + return state in [cls.PRE_FAULT, cls.FAULT] + + @classmethod + def is_off(cls, state): + return state in [cls.OFF] + +class ACCState(object): + # Possible state of the ACC system, following the DI_cruiseState naming + # convention. + OFF = 0 # Disabled by UI. + STANDBY = 1 # Ready to be enaged. + ENABLED = 2 # Engaged. + NOT_READY = 9 # Not ready to be engaged due to the state of the car. + +class ACCMode(object): + FOLLOW = "ACC" + AUTO = "ACC AUTO" + OFF = "ACC OFF" + + PEDAL_FOLLOW = "PEDAL" + PEDAL_MPC = "PEDAL MPC" + PEDAL_OFF = "PEDAL OFF" + + + # This static map is filled just below the class (so that it can contain + # ACCMode objects) + ACC_MODES = None + + @classmethod + def get(cls, name): + if name in cls.ACC_MODES: + return cls.ACC_MODES[name] + else: + return cls.ACC_MODES.values()[0] + + def __init__(self, name, autoresume, state, next_mode): + self.name = name + self.autoresume = autoresume + self.state = state + self.next_mode = next_mode + + def next(self): + return self.get(self.next_mode) + +ACCMode.ACC_MODES = { + # Cruise-control based modes + ACCMode.FOLLOW: ACCMode(name=ACCMode.FOLLOW, autoresume=False, state=ACCState.STANDBY, next_mode=ACCMode.AUTO), + ACCMode.AUTO: ACCMode(name=ACCMode.AUTO, autoresume=True, state=ACCState.STANDBY, next_mode=ACCMode.OFF), + ACCMode.OFF: ACCMode(name=ACCMode.OFF, autoresume=False, state=ACCState.OFF, next_mode=ACCMode.FOLLOW), + + # Pedal-based modes + ACCMode.PEDAL_FOLLOW: ACCMode(name=ACCMode.PEDAL_FOLLOW, autoresume=False, state=ACCState.STANDBY, next_mode=ACCMode.PEDAL_MPC), + ACCMode.PEDAL_MPC: ACCMode(name=ACCMode.PEDAL_MPC, autoresume=False, state=ACCState.STANDBY, next_mode=ACCMode.PEDAL_OFF), + ACCMode.PEDAL_OFF: ACCMode(name=ACCMode.PEDAL_MPC, autoresume=False, state=ACCState.OFF, next_mode=ACCMode.PEDAL_FOLLOW) +} \ No newline at end of file diff --git a/selfdrive/car/toyota/.gitignore b/selfdrive/car/toyota/.gitignore new file mode 100644 index 00000000000000..89fa7bc7daf83e --- /dev/null +++ b/selfdrive/car/toyota/.gitignore @@ -0,0 +1,2 @@ +buttons.msg +buttons.cc.msg diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index b0c6bf5209f63b..9866f023d870b3 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -7,6 +7,7 @@ create_fcw_command from selfdrive.car.toyota.values import ECU, STATIC_MSGS from selfdrive.can.packer import CANPacker +from selfdrive.car.modules.ALCA_module import ALCAController VisualAlert = car.CarControl.HUDControl.VisualAlert AudibleAlert = car.CarControl.HUDControl.AudibleAlert @@ -118,11 +119,17 @@ def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu, enable_ if enable_camera: self.fake_ecus.add(ECU.CAM) if enable_dsu: self.fake_ecus.add(ECU.DSU) if enable_apg: self.fake_ecus.add(ECU.APGS) + self.ALCA = ALCAController(self,True,False) # Enabled True and SteerByAngle only False self.packer = CANPacker(dbc_name) def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera): + #update custom UI buttons and alerts + CS.UE.update_custom_ui() + if (frame % 1000 == 0): + CS.cstm_btns.send_button_info() + CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder,CS.cstm_btns.car_name) # *** compute control surfaces *** @@ -130,9 +137,19 @@ def update(self, sendcan, enabled, CS, frame, actuators, apply_accel = actuators.gas - actuators.brake apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled) apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) - + # Get the angle from ALCA. + alca_enabled = False + alca_steer = 0. + alca_angle = 0. + turn_signal_needed = 0 + # Update ALCA status and custom button every 0.1 sec. + if self.ALCA.pid == None: + self.ALCA.set_pid(CS) + if (frame % 10 == 0): + self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0) # steer torque - apply_steer = int(round(actuators.steer * STEER_MAX)) + alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(enabled, CS, frame, actuators) + apply_steer = int(round(alca_steer * STEER_MAX)) max_lim = min(max(CS.steer_torque_motor + STEER_ERROR_MAX, STEER_ERROR_MAX), STEER_MAX) min_lim = max(min(CS.steer_torque_motor - STEER_ERROR_MAX, -STEER_ERROR_MAX), -STEER_MAX) @@ -162,9 +179,11 @@ def update(self, sendcan, enabled, CS, frame, actuators, ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter) #print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active + # steer angle if self.steer_angle_enabled and CS.ipas_active: - apply_angle = actuators.steerAngle + + apply_angle = alca_angle angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V) apply_angle = clip(apply_angle, -angle_lim, angle_lim) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 71d33b0ab7c2a2..0939913b69de4c 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -3,7 +3,10 @@ from selfdrive.can.parser import CANParser, CANDefine from selfdrive.config import Conversions as CV from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD - +from common.kalman.simple_kalman import KF1D +from selfdrive.car.modules.UIBT_module import UIButtons,UIButton +from selfdrive.car.modules.UIEV_module import UIEvents +import numpy as np def parse_gear_shifter(gear, vals): val_to_capnp = {'P': 'park', 'R': 'reverse', 'N': 'neutral', @@ -78,12 +81,76 @@ def get_cam_can_parser(CP): class CarState(object): def __init__(self, CP): - + #labels for buttons + self.btns_init = [["alca","ALC",["MadMax","Normal","Wifey"]], \ + ["","",[""]], \ + ["","",[""]], \ + ["sound","SND",[""]], \ + ["", "",[""]], \ + ["", "", [""]]] + #if (CP.carFingerprint == CAR.MODELS): + # ALCA PARAMS + # max REAL delta angle for correction vs actuator + self.CL_MAX_ANGLE_DELTA_BP = [10., 44.] + self.CL_MAX_ANGLE_DELTA = [1.8, .3] + + # adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother + self.CL_ADJUST_FACTOR_BP = [10., 44.] + self.CL_ADJUST_FACTOR = [16. , 8.] + + + # reenrey angle when to let go + self.CL_REENTRY_ANGLE_BP = [10., 44.] + self.CL_REENTRY_ANGLE = [5. , 5.] + + # a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line + self.CL_LANE_DETECT_BP = [10., 44.] + self.CL_LANE_DETECT_FACTOR = [1.5, 1.5] + + self.CL_LANE_PASS_BP = [10., 20., 44.] + self.CL_LANE_PASS_TIME = [40.,10., 3.] + + # change lane delta angles and other params + self.CL_MAXD_BP = [10., 32., 44.] + self.CL_MAXD_A = [.358, 0.084, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75 + + self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s + + # do not turn if actuator wants more than x deg for going straight; this should be interp based on speed + self.CL_MAX_A_BP = [10., 44.] + self.CL_MAX_A = [10., 10.] + + # define limits for angle change every 0.1 s + # we need to force correction above 10 deg but less than 20 + # anything more means we are going to steep or not enough in a turn + self.CL_MAX_ACTUATOR_DELTA = 2. + self.CL_MIN_ACTUATOR_DELTA = 0. + self.CL_CORRECTION_FACTOR = 1. + + #duration after we cross the line until we release is a factor of speed + self.CL_TIMEA_BP = [10., 32., 44.] + self.CL_TIMEA_T = [0.7 ,0.30, 0.20] + + #duration to wait (in seconds) with blinkers on before starting to turn + self.CL_WAIT_BEFORE_START = 1 + #END OF ALCA PARAMS + self.CP = CP self.can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR'] self.left_blinker_on = 0 self.right_blinker_on = 0 + #BB UIEvents + self.UE = UIEvents(self) + + #BB variable for custom buttons + self.cstm_btns = UIButtons(self,"Toyota","toyota") + + #BB pid holder for ALCA + self.pid = None + + #BB custom message counter + self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero # initialize can parser self.car_fingerprint = CP.carFingerprint @@ -97,6 +164,7 @@ def __init__(self, CP): C=np.matrix([1.0, 0.0]), K=np.matrix([[0.12287673], [0.29666309]])) self.v_ego = 0.0 + def update(self, cp, cp_cam): # copy can_valid diff --git a/selfdrive/common/touch.c b/selfdrive/common/touch.c index f21fac8f07afd5..3e8c32ad70f072 100644 --- a/selfdrive/common/touch.c +++ b/selfdrive/common/touch.c @@ -75,8 +75,8 @@ int touch_poll(TouchState *s, int* out_x, int* out_y, int timeout) { s->last_x = event.value; } else if (event.code == ABS_MT_POSITION_Y) { s->last_y = event.value; - } - up = true; + } else + up = true; break; default: break; diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 0a96c233516499..de153ffd46ba85 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -274,7 +274,7 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, v_cruise_kph, plan.vTarget, plan.vTargetFuture, plan.aTarget, CP, PL.lead_1) # Steering PID loop and lateral MPC - actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle, + actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringPressed, plan.dPoly, angle_offset, CP, VM, PL) # Send a "steering required alert" if saturation count has reached the limit @@ -365,7 +365,7 @@ def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, ac "jerkFactor": float(plan.jerkFactor), "angleOffset": float(angle_offset), "gpsPlannerActive": plan.gpsPlannerActive, - "cumLagMs": -rk.remaining * 1000., + "cumLagMs": -rk.remaining*1000., } live100.send(dat.to_bytes()) @@ -416,7 +416,7 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): # No sendcan if passive if not passive: - sendcan = messaging.pub_sock(context, service_list['sendcan'].port) + sendcan = messaging.pub_sock(context, service_list['sendcan'].port) else: sendcan = None @@ -472,7 +472,7 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): mismatch_counter = 0 low_battery = False - rk = Ratekeeper(rate, print_delay_threshold=2. / 1000) + rk = Ratekeeper(rate, print_delay_threshold=2./1000) # Read angle offset from previous drive, fallback to default angle_offset = default_bias diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 81a371f783ca6c..aee0a25d008b79 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -11,16 +11,22 @@ _DT = 0.01 # 100Hz _DT_MPC = 0.05 # 20Hz - def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay): - states[0].x = v_ego * delay + states[0].x = v_ego * delay + 2 states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay return states - def get_steer_max(CP, v_ego): return interp(v_ego, CP.steerMaxBP, CP.steerMaxV) +def apply_deadzone(angle, deadzone): + if angle > deadzone: + angle -= deadzone + elif angle < -deadzone: + angle += deadzone + else: + angle = 0. + return angle class LatControl(object): def __init__(self, CP): @@ -29,6 +35,24 @@ def __init__(self, CP): k_f=CP.steerKf, pos_limit=1.0) self.last_cloudlog_t = 0.0 self.setup_mpc(CP.steerRateCost) + self.smooth_factor = 2.0 * CP.steerActuatorDelay / _DT # Multiplier for inductive component (feed forward) + self.projection_factor = CP.steerActuatorDelay # Mutiplier for reactive component (PI) + self.accel_limit = 2.0 # Desired acceleration limit to prevent "whip steer" (resistive component) + self.ff_angle_factor = 0.5 # Kf multiplier for angle-based feed forward + self.ff_rate_factor = 5.0 # Kf multiplier for rate-based feed forward + self.prev_angle_rate = 0 + self.feed_forward = 0.0 + self.steerActuatorDelay = CP.steerActuatorDelay + self.last_mpc_ts = 0.0 + self.angle_steers_des = 0.0 + self.angle_steers_des_mpc = 0.0 + self.angle_steers_des_time = 0.0 + self.avg_angle_steers = 0.0 + self.projected_angle_steers = 0.0 + + self.frames = 0 + self.curvature_factor = 0.0 + self.mpc_frame = 0 def setup_mpc(self, steer_rate_cost): self.libmpc = libmpc_py.libmpc @@ -36,6 +60,8 @@ def setup_mpc(self, steer_rate_cost): self.mpc_solution = libmpc_py.ffi.new("log_t *") self.cur_state = libmpc_py.ffi.new("state_t *") + self.mpc_angles = [0.0, 0.0, 0.0] + self.mpc_times = [0.0, 0.0, 0.0] self.mpc_updated = False self.mpc_nans = False self.cur_state[0].x = 0.0 @@ -43,78 +69,121 @@ def setup_mpc(self, steer_rate_cost): self.cur_state[0].psi = 0.0 self.cur_state[0].delta = 0.0 - self.last_mpc_ts = 0.0 - self.angle_steers_des = 0.0 - self.angle_steers_des_mpc = 0.0 - self.angle_steers_des_prev = 0.0 - self.angle_steers_des_time = 0.0 - def reset(self): self.pid.reset() - def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, CP, VM, PL): - cur_time = sec_since_boot() + def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly, angle_offset, CP, VM, PL): self.mpc_updated = False + + # Use steering rate from the last 2 samples to estimate acceleration for a more realistic future steering rate + accelerated_angle_rate = 2.0 * angle_rate - self.prev_angle_rate + + # Determine future angle steers using accelerated steer rate + self.projected_angle_steers = float(angle_steers) + self.projection_factor * float(accelerated_angle_rate) + # TODO: this creates issues in replay when rewinding time: mpc won't run if self.last_mpc_ts < PL.last_md_ts: + cur_time = sec_since_boot() self.last_mpc_ts = PL.last_md_ts - self.angle_steers_des_prev = self.angle_steers_des_mpc - curvature_factor = VM.curvature_factor(v_ego) + self.curvature_factor = VM.curvature_factor(v_ego) - l_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.l_poly)) - r_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.r_poly)) - p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly)) + # Determine a proper delay time that includes the model's processing time, which is variable + plan_age = cur_time - float(self.last_mpc_ts / 1000000000.0) + total_delay = CP.steerActuatorDelay + plan_age - # account for actuation delay - self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, CP.steerRatio, CP.steerActuatorDelay) + self.l_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.l_poly)) + self.r_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.r_poly)) + self.p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly)) + + # account for actuation delay and the age of the plan + self.cur_state = calc_states_after_delay(self.cur_state, v_ego, self.projected_angle_steers, self.curvature_factor, CP.steerRatio, total_delay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, - l_poly, r_poly, p_poly, - PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego_mpc, PL.PP.lane_width) - - # reset to current steer angle if not active or overriding - if active: - delta_desired = self.mpc_solution[0].delta[1] - else: - delta_desired = math.radians(angle_steers - angle_offset) / CP.steerRatio - - self.cur_state[0].delta = delta_desired + self.l_poly, self.r_poly, self.p_poly, + PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, self.curvature_factor, v_ego_mpc, PL.PP.lane_width) - self.angle_steers_des_mpc = float(math.degrees(delta_desired * CP.steerRatio) + angle_offset) - self.angle_steers_des_time = cur_time self.mpc_updated = True # Check for infeasable MPC solution self.mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) - t = sec_since_boot() - if self.mpc_nans: + if not self.mpc_nans: + self.mpc_angles = [self.angle_steers_des, + float(math.degrees(self.mpc_solution[0].delta[1] * CP.steerRatio) + angle_offset), + float(math.degrees(self.mpc_solution[0].delta[2] * CP.steerRatio) + angle_offset)] + + self.mpc_times = [self.angle_steers_des_time, + float(self.last_mpc_ts / 1000000000.0) + _DT_MPC, + float(self.last_mpc_ts / 1000000000.0) + _DT_MPC + _DT_MPC] + + self.angle_steers_des_mpc = self.mpc_angles[1] + else: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost) self.cur_state[0].delta = math.radians(angle_steers) / CP.steerRatio - if t > self.last_cloudlog_t + 5.0: - self.last_cloudlog_t = t + if cur_time > self.last_cloudlog_t + 5.0: + self.last_cloudlog_t = cur_time cloudlog.warning("Lateral mpc - nan: True") if v_ego < 0.3 or not active: output_steer = 0.0 + self.feed_forward = 0.0 self.pid.reset() + self.angle_steers_des = angle_steers + self.avg_angle_steers = angle_steers + self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / CP.steerRatio else: - # TODO: ideally we should interp, but for tuning reasons we keep the mpc solution - # constant for 0.05s. - #dt = min(cur_time - self.angle_steers_des_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps - #self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev) - self.angle_steers_des = self.angle_steers_des_mpc + cur_time = sec_since_boot() + + # Interpolate desired angle between MPC updates + self.angle_steers_des = np.interp(cur_time, self.mpc_times, self.mpc_angles) + self.angle_steers_des_time = cur_time + self.avg_angle_steers = (4.0 * self.avg_angle_steers + angle_steers) / 5.0 + self.cur_state[0].delta = math.radians(self.angle_steers_des - angle_offset) / CP.steerRatio + + # Determine the target steer rate for desired angle, but prevent the acceleration limit from being exceeded + # Restricting the steer rate creates the resistive component needed for resonance + restricted_steer_rate = np.clip(self.angle_steers_des - float(angle_steers) , float(accelerated_angle_rate) - self.accel_limit, float(accelerated_angle_rate) + self.accel_limit) + + # Determine projected desired angle that is within the acceleration limit (prevent the steering wheel from jerking) + projected_angle_steers_des = self.angle_steers_des + self.projection_factor * restricted_steer_rate + steers_max = get_steer_max(CP, v_ego) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max - steer_feedforward = self.angle_steers_des # feedforward desired angle + if CP.steerControlType == car.CarParams.SteerControlType.torque: - steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel) + # Decide which feed forward mode should be used (angle or rate). Use more dominant mode, and only if conditions are met + # Spread feed forward out over a period of time to make it more inductive (for resonance) + if abs(self.ff_rate_factor * float(restricted_steer_rate)) > abs(self.ff_angle_factor * float(self.angle_steers_des) - float(angle_offset)) - 0.5 \ + and (abs(float(restricted_steer_rate)) > abs(accelerated_angle_rate) or (float(restricted_steer_rate) < 0) != (accelerated_angle_rate < 0)) \ + and (float(restricted_steer_rate) < 0) == (float(self.angle_steers_des) - float(angle_offset) - 0.5 < 0): + ff_type = "r" + self.feed_forward = (((self.smooth_factor - 1.) * self.feed_forward) + self.ff_rate_factor * v_ego**2 * float(restricted_steer_rate)) / self.smooth_factor + elif abs(self.angle_steers_des - float(angle_offset)) > 0.5: + ff_type = "a" + self.feed_forward = (((self.smooth_factor - 1.) * self.feed_forward) + self.ff_angle_factor * v_ego**2 * float(apply_deadzone(float(self.angle_steers_des) - float(angle_offset), 0.5))) / self.smooth_factor + else: + ff_type = "r" + self.feed_forward = (((self.smooth_factor - 1.) * self.feed_forward) + 0.0) / self.smooth_factor + else: + self.feed_forward = self.angle_steers_des # feedforward desired angle deadzone = 0.0 - output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, - feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone) + + # Use projected desired and actual angles instead of "current" values, in order to make PI more reactive (for resonance) + output_steer = self.pid.update(projected_angle_steers_des, self.projected_angle_steers, check_saturation=(v_ego > 10), override=steer_override, + feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone) + + # Hide angle error if being overriden + if steer_override: + self.projected_angle_steers = self.mpc_angles[1] + self.avg_angle_steers = self.mpc_angles[1] self.sat_flag = self.pid.saturated - return output_steer, float(self.angle_steers_des) + self.prev_angle_rate = angle_rate + + if CP.steerControlType == car.CarParams.SteerControlType.torque: + return output_steer, float(self.angle_steers_des_mpc) + else: + return float(self.angle_steers_des), float(self.angle_steers_des_mpc) \ No newline at end of file diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 092edd27a1c64d..6a20915fec1184 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -127,4 +127,4 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_ final_gas = clip(output_gb, 0., gas_max) final_brake = -clip(output_gb, -brake_max, 0.) - return final_gas, final_brake + return final_gas, final_brake \ No newline at end of file diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 178b5e6dbb2de4..c61afff8832797 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -1,3 +1,5 @@ +import numpy as np +import math from common.numpy_fast import interp from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv @@ -12,19 +14,31 @@ def __init__(self): self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1 self._path_pinv = compute_path_pinv() - self.lane_width_estimate = 3.7 + self.lane_width_estimate = 3.2 self.lane_width_certainty = 1.0 - self.lane_width = 3.7 + self.lane_width = 3.2 - def update(self, v_ego, md): + def update(self, v_ego, md, LaC=None): if md is not None: p_poly = model_polyfit(md.model.path.points, self._path_pinv) # predicted path l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv) # left line r_poly = model_polyfit(md.model.rightLane.points, self._path_pinv) # right line + try: + if LaC is not None and LaC.angle_steers_des_mpc != 0.0: + angle_error = LaC.angle_steers_des - (0.05 * LaC.avg_angle_steers + LaC.steerActuatorDelay * LaC.projected_angle_steers) / (LaC.steerActuatorDelay + 0.05) + else: + angle_error = 0.0 + if angle_error != 0.0: + lateral_error = 1.0 * np.clip(v_ego * (LaC.steerActuatorDelay + 0.05) * math.tan(math.radians(angle_error)), -0.5, 0.5) + else: + lateral_error = 0.0 + except: + lateral_error = 0.0 + # only offset left and right lane lines; offsetting p_poly does not make sense - l_poly[3] += CAMERA_OFFSET - r_poly[3] += CAMERA_OFFSET + l_poly[3] += CAMERA_OFFSET - lateral_error + r_poly[3] += CAMERA_OFFSET - lateral_error p_prob = 1. # model does not tell this probability yet, so set to 1 for now l_prob = md.model.leftLane.prob # left line prob @@ -59,4 +73,4 @@ def update(self, v_ego, md): self.l_prob = l_prob self.p_poly = p_poly - self.p_prob = p_prob + self.p_prob = p_prob \ No newline at end of file diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index b68dc3810eef4b..57289bb021c8de 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -88,4 +88,4 @@ def update(self, setpoint, measurement, speed=0.0, check_saturation=True, overri self.saturated = False self.control = clip(control, self.neg_limit, self.pos_limit) - return self.control + return self.control \ No newline at end of file diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index b986740113904d..8d860bd649ca87 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -364,7 +364,7 @@ def update(self, CS, CP, VM, LaC, LoC, v_cruise_kph, force_slow_decel): self.last_model = cur_time self.model_dead = False - self.PP.update(CS.vEgo, md) + self.PP.update(CS.vEgo, md, LaC) if self.last_gps_planner_plan is not None: plan = self.last_gps_planner_plan.gpsPlannerPlan @@ -513,4 +513,4 @@ def update(self, CS, CP, VM, LaC, LoC, v_cruise_kph, force_slow_decel): plan_send.plan.fcw = fcw self.plan.send(plan_send.to_bytes()) - return plan_send + return plan_send \ No newline at end of file diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index b909b4952af7be..1e3ee4f0e7d5dc 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -52,7 +52,7 @@ def radard_thread(gctx=None): # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) - mocked = CP.carName == "mock" + mocked = CP.carName == "mock" or CP.carName == "tesla" VM = VehicleModel(CP) cloudlog.info("radard got CarParams") @@ -143,8 +143,9 @@ def radard_thread(gctx=None): ekfv.predict(tsv) # When changing lanes the distance to the lead car can suddenly change, - # which makes the Kalman filter output large relative acceleration - if mocked and abs(PP.lead_dist - ekfv.state[XV]) > 2.0: + # which makes the Kalman filter output large relative acceleration. + # OpenPilot 0.5.6 set this to 2.0 which seems a little too low. + if mocked and abs(PP.lead_dist - ekfv.state[XV]) > 2.2: ekfv.state[XV] = PP.lead_dist ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init])) ekfv.state[SPEEDV] = 0. diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 90753644d3cdb6..8127de28e8f37d 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -347,8 +347,8 @@ def manager_thread(): kill_managed_process(p) # check the status of all processes, did any of them die? - for p in running: - cloudlog.debug(" running %s %s" % (p, running[p])) + #for p in running: + # cloudlog.debug(" running %s %s" % (p, running[p])) # is this still needed? if params.get("DoUninstall") == "1": diff --git a/selfdrive/service_list.yaml b/selfdrive/service_list.yaml index 7a875d219b638a..4a16a06654daf9 100644 --- a/selfdrive/service_list.yaml +++ b/selfdrive/service_list.yaml @@ -77,6 +77,15 @@ testModel: [8040, false] testLiveLocation: [8045, false] testJoystick: [8056, false] +#ui Events +uiButtonInfo: [8201, true] +uiCustomAlert: [8202, true] +uiSetCar: [8203, true] +uiButtonStatus: [8204, true] +uiPlaySound: [8205, true] +uiUpdate: [8206, true] + + # 8080 is reserved for slave testing daemon # 8762 is reserved for logserver diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py index 7f39d310d7afd6..61d33b20255355 100755 --- a/selfdrive/thermald.py +++ b/selfdrive/thermald.py @@ -299,7 +299,7 @@ def thermald_thread(): msg.thermal.thermalStatus = thermal_status thermal_sock.send(msg.to_bytes()) - print msg + #print msg # report to server once per minute if (count%60) == 0: diff --git a/selfdrive/ui/Makefile b/selfdrive/ui/Makefile index a78a48cf2a9193..8fffb51be9ee8d 100644 --- a/selfdrive/ui/Makefile +++ b/selfdrive/ui/Makefile @@ -20,7 +20,7 @@ ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \ CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include CEREAL_LIBS = -L$(PHONELIBS)/capnp-c/aarch64/lib -l:libcapn.a -CEREAL_OBJS = ../../cereal/gen/c/log.capnp.o +CEREAL_OBJS = ../../cereal/gen/c/log.capnp.o ../../cereal/gen/c/ui.capnp.o NANOVG_FLAGS = -I$(PHONELIBS)/nanovg JSON_FLAGS = -I$(PHONELIBS)/json/src diff --git a/selfdrive/ui/bbui.h b/selfdrive/ui/bbui.h new file mode 100644 index 00000000000000..db48592c1202fe --- /dev/null +++ b/selfdrive/ui/bbui.h @@ -0,0 +1,1088 @@ +#include "cereal/gen/c/ui.capnp.h" + + + + +vec3 bb_car_space_to_full_frame(const UIState *s, vec4 car_space_projective) { + const UIScene *scene = &s->scene; + + // We'll call the car space point p. + // First project into normalized image coordinates with the extrinsics matrix. + const vec4 Ep4 = matvecmul(scene->extrinsic_matrix, car_space_projective); + + // The last entry is zero because of how we store E (to use matvecmul). + const vec3 Ep = {{Ep4.v[0], Ep4.v[1], Ep4.v[2]}}; + const vec3 KEp = matvecmul3(intrinsic_matrix, Ep); + + // Project. + const vec3 p_image = {{KEp.v[0] / KEp.v[2], KEp.v[1] / KEp.v[2], 1.}}; + return p_image; +} + + + +void bb_ui_draw_vision_alert( UIState *s, int va_size, int va_color, + const char* va_text1, const char* va_text2) { + const UIScene *scene = &s->scene; + int ui_viz_rx = scene->ui_viz_rx; + int ui_viz_rw = scene->ui_viz_rw; + bool hasSidebar = !s->scene.uilayout_sidebarcollapsed; + bool mapEnabled = s->scene.uilayout_mapenabled; + bool longAlert1 = strlen(va_text1) > 15; + + const uint8_t *color = alert_colors[va_color]; + const int alr_s = alert_sizes[va_size]; + const int alr_x = ui_viz_rx-(mapEnabled?(hasSidebar?nav_w:(nav_ww)):0)-bdr_s; + const int alr_w = ui_viz_rw+(mapEnabled?(hasSidebar?nav_w:(nav_ww)):0)+(bdr_s*2); + const int alr_h = alr_s+(va_size==ALERTSIZE_NONE?0:bdr_s); + const int alr_y = vwp_h-alr_h; + + nvgBeginPath(s->vg); + nvgRect(s->vg, alr_x, alr_y, alr_w, alr_h); + nvgFillColor(s->vg, nvgRGBA(color[0],color[1],color[2],(color[3]*s->alert_blinking_alpha))); + nvgFill(s->vg); + + nvgBeginPath(s->vg); + NVGpaint gradient = nvgLinearGradient(s->vg, alr_x, alr_y, alr_x, alr_y+alr_h, + nvgRGBAf(0.0,0.0,0.0,0.05), nvgRGBAf(0.0,0.0,0.0,0.35)); + nvgFillPaint(s->vg, gradient); + nvgRect(s->vg, alr_x, alr_y, alr_w, alr_h); + nvgFill(s->vg); + + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + + if (va_size == ALERTSIZE_SMALL) { + nvgFontFace(s->vg, "sans-semibold"); + nvgFontSize(s->vg, 40*2.5); + nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+15, va_text1, NULL); + } else if (va_size== ALERTSIZE_MID) { + nvgFontFace(s->vg, "sans-bold"); + nvgFontSize(s->vg, 48*2.5); + nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2-45, va_text1, NULL); + nvgFontFace(s->vg, "sans-regular"); + nvgFontSize(s->vg, 36*2.5); + nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+75, va_text2, NULL); + } else if (va_size== ALERTSIZE_FULL) { + nvgFontSize(s->vg, (longAlert1?72:96)*2.5); + nvgFontFace(s->vg, "sans-bold"); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); + nvgTextBox(s->vg, alr_x, alr_y+(longAlert1?360:420), alr_w-60, va_text1, NULL); + nvgFontSize(s->vg, 48*2.5); + nvgFontFace(s->vg, "sans-regular"); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM); + nvgTextBox(s->vg, alr_x, alr_h-(longAlert1?300:360), alr_w-60, va_text2, NULL); + } +} + + + +void bb_ui_draw_car( UIState *s) { + // replaces the draw_chevron function when button in mid position + //static void draw_chevron(UIState *s, float x_in, float y_in, float sz, + // NVGcolor fillColor, NVGcolor glowColor) { + if (!s->scene.lead_status) { + //no lead car to draw + return; + } + const UIScene *scene = &s->scene; + float x_in = scene->lead_d_rel+2.7; + float y_in = scene->lead_y_rel; + float sz = 1.0; + + nvgSave(s->vg); + nvgTranslate(s->vg, 240.0f, 0.0); + nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); + nvgScale(s->vg, 2.0, 2.0); + nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height); + + const vec4 p_car_space = (vec4){{x_in, y_in, 0., 1.}}; + const vec3 p_full_frame = bb_car_space_to_full_frame(s, p_car_space); + + float x = p_full_frame.v[0]; + float y = p_full_frame.v[1]; + + // glow + + float bbsz =0.1* 0.1 + 0.6 * (180-x_in*3.3-20)/180; + if (bbsz < 0.1) bbsz = 0.1; + if (bbsz > 0.6) bbsz = 0.6; + float car_alpha = .8; + float car_w = 750 * bbsz; + float car_h = 531 * bbsz; + float car_y = y - car_h/2; + float car_x = x - car_w/2; + + nvgBeginPath(s->vg); + NVGpaint imgPaint = nvgImagePattern(s->vg, car_x, car_y, + car_w, car_h, 0, s->b.img_car, car_alpha); + nvgRect(s->vg, car_x, car_y, car_w, car_h); + nvgFillPaint(s->vg, imgPaint); + nvgFill(s->vg); + nvgRestore(s->vg); +} + +void bb_draw_lane_fill ( UIState *s) { + + const UIScene *scene = &s->scene; + + nvgSave(s->vg); + nvgTranslate(s->vg, 240.0f, 0.0); // rgb-box space + nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); // zoom 2x + nvgScale(s->vg, 2.0, 2.0); + nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height); + nvgBeginPath(s->vg); + + //BB let the magic begin + //define variables + PathData path; + float *points; + float off; + NVGcolor bb_color = nvgRGBA(138, 140, 142,180); + bool started = false; + bool is_ghost = true; + //left lane, first init + path = scene->model.left_lane; + points = path.points; + off = 0.025*path.prob; + //draw left + for (int i=0; i<49; i++) { + float px = (float)i; + float py = points[i] - off; + vec4 p_car_space = (vec4){{px, py, 0., 1.}}; + vec3 p_full_frame = bb_car_space_to_full_frame(s, p_car_space); + float x = p_full_frame.v[0]; + float y = p_full_frame.v[1]; + if (x < 0 || y < 0.) { + continue; + } + if (!started) { + nvgMoveTo(s->vg, x, y); + started = true; + } else { + nvgLineTo(s->vg, x, y); + } + } + //right lane, first init + path = scene->model.right_lane; + points = path.points; + off = 0.025*path.prob; + //draw right + for (int i=49; i>0; i--) { + float px = (float)i; + float py = is_ghost?(points[i]-off):(points[i]+off); + vec4 p_car_space = (vec4){{px, py, 0., 1.}}; + vec3 p_full_frame = bb_car_space_to_full_frame(s, p_car_space); + float x = p_full_frame.v[0]; + float y = p_full_frame.v[1]; + if (x < 0 || y < 0.) { + continue; + } + nvgLineTo(s->vg, x, y); + } + + nvgClosePath(s->vg); + nvgFillColor(s->vg, bb_color); + nvgFill(s->vg); + nvgRestore(s->vg); + +} + + + + + +long bb_currentTimeInMilis() { + struct timespec res; + clock_gettime(CLOCK_MONOTONIC, &res); + return (res.tv_sec * 1000) + res.tv_nsec/1000000; +} + +int bb_get_status( UIState *s) { + //BB select status based on main s->status w/ priority over s->b.custom_message_status + int bb_status = -1; + if ((s->status == STATUS_ENGAGED) && (s->b.custom_message_status > STATUS_ENGAGED)) { + bb_status = s->b.custom_message_status; + } else { + bb_status = s->status; + } + return bb_status; +} + +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, + NVGcolor bb_valueColor, NVGcolor bb_labelColor, NVGcolor bb_uomColor, + int bb_valueFontSize, int bb_labelFontSize, int bb_uomFontSize ) { + const UIScene *scene = &s->scene; + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + int dx = 0; + if (strlen(bb_uom) > 0) { + dx = (int)(bb_uomFontSize*2.5/2); + } + //print value + nvgFontFace(s->vg, "sans-semibold"); + nvgFontSize(s->vg, bb_valueFontSize*2.5); + nvgFillColor(s->vg, bb_valueColor); + nvgText(s->vg, bb_x-dx/2, bb_y+ (int)(bb_valueFontSize*2.5)+5, bb_value, NULL); + //print label + nvgFontFace(s->vg, "sans-regular"); + nvgFontSize(s->vg, bb_labelFontSize*2.5); + nvgFillColor(s->vg, bb_labelColor); + nvgText(s->vg, bb_x, bb_y + (int)(bb_valueFontSize*2.5)+5 + (int)(bb_labelFontSize*2.5)+5, bb_label, NULL); + //print uom + if (strlen(bb_uom) > 0) { + nvgSave(s->vg); + int rx =bb_x + bb_uom_dx + bb_valueFontSize -3; + int ry = bb_y + (int)(bb_valueFontSize*2.5/2)+25; + nvgTranslate(s->vg,rx,ry); + nvgRotate(s->vg, -1.5708); //-90deg in radians + nvgFontFace(s->vg, "sans-regular"); + nvgFontSize(s->vg, (int)(bb_uomFontSize*2.5)); + nvgFillColor(s->vg, bb_uomColor); + nvgText(s->vg, 0, 0, bb_uom, NULL); + nvgRestore(s->vg); + } + return (int)((bb_valueFontSize + bb_labelFontSize)*2.5) + 5; +} + + +bool bb_handle_ui_touch( UIState *s, int touch_x, int touch_y) { + for(int i=0; i<6; i++) { + if (s->b.btns_r[i] > 0) { + if ((abs(touch_x - s->b.btns_x[i]) < s->b.btns_r[i]) && (abs(touch_y - s->b.btns_y[i]) < s->b.btns_r[i])) { + //found it; change the status + if (s->b.btns_status[i] > 0) { + s->b.btns_status[i] = 0; + } else { + s->b.btns_status[i] = 1; + } + //now let's send the cereal + + struct capn c; + capn_init_malloc(&c); + struct capn_ptr cr = capn_root(&c); + struct capn_segment *cs = cr.seg; + + struct cereal_UIButtonStatus btn_d; + btn_d.btnId = i; + btn_d.btnStatus = s->b.btns_status[i]; + + cereal_UIButtonStatus_ptr btn_p = cereal_new_UIButtonStatus(cs); + + cereal_write_UIButtonStatus(&btn_d, btn_p); + int setp_ret = capn_setp(capn_root(&c), 0, btn_p.p); + assert(setp_ret == 0); + + uint8_t buf[4096]; + ssize_t rs = capn_write_mem(&c, buf, sizeof(buf), 0); + + capn_free(&c); + + zmq_send(s->b.uiButtonStatus_sock_raw, buf,rs,0); + + return true; + } + } + } + return false; +}; + +int bb_get_button_status( UIState *s, char *btn_name) { + int ret_status = -1; + for (int i = 0; i< 6; i++) { + if (strcmp(s->b.btns[i].btn_name,btn_name)==0) { + ret_status = s->b.btns_status[i]; + } + } + return ret_status; +} + +void bb_draw_button( UIState *s, int btn_id) { + const UIScene *scene = &s->scene; + + int viz_button_x = 0; + int viz_button_y = (box_y + (bdr_s*1.5)) + 20; + int viz_button_w = 140; + int viz_button_h = 140; + + char *btn_text, *btn_text2; + + int delta_x = viz_button_w * 1.1; + int delta_y = 0; + int dx1 = 200; + int dx2 = 190; + int dy = 0; + + if (s->b.tri_state_switch ==2) { + delta_y = delta_x; + delta_x = 0; + dx1 = 20; + dx2 = 160; + dy = 200; + } + + if (btn_id >2) { + viz_button_x = scene->ui_viz_rx + scene->ui_viz_rw - (bdr_s*2) -dx2; + viz_button_x -= (6-btn_id) * delta_x ; + viz_button_y += (btn_id-3) * delta_y + dy; + + } else { + viz_button_x = scene->ui_viz_rx + (bdr_s*2) + dx1; + viz_button_x += (btn_id) * delta_x; + viz_button_y += btn_id * delta_y + dy; + } + + + btn_text = s->b.btns[btn_id].btn_label; + btn_text2 = s->b.btns[btn_id].btn_label2; + + if (strcmp(btn_text,"")==0) { + s->b.btns_r[btn_id] = 0; + } else { + s->b.btns_r[btn_id]= (int)((viz_button_w + viz_button_h)/4); + } + s->b.btns_x[btn_id]=viz_button_x + s->b.btns_r[btn_id]; + s->b.btns_y[btn_id]=viz_button_y + s->b.btns_r[btn_id]; + if (s->b.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->b.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->b.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->b.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->b.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, viz_button_y + 112, 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, viz_button_y + 85,btn_text, NULL); +} + +void bb_draw_buttons( UIState *s) { + for (int i = 0; i < 6; i++) { + bb_draw_button(s,i); + } +} + +void bb_ui_draw_custom_alert( UIState *s) { + if ((strlen(s->b.custom_message) > 0) && (strlen(s->scene.alert_text1)==0)){ + if (!((bb_get_button_status(s,"msg") == 0) && (s->b.custom_message_status<=3))) { + bb_ui_draw_vision_alert(s, ALERTSIZE_SMALL, s->b.custom_message_status, + s->b.custom_message,""); + + } + } +} + + +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); + int bb_ry = bb_y; + int bb_h = 5; + NVGcolor lab_color = nvgRGBA(255, 255, 255, 200); + NVGcolor uom_color = nvgRGBA(255, 255, 255, 200); + int value_fontSize=30; + int label_fontSize=15; + int uom_fontSize = 15; + int bb_uom_dx = (int)(bb_w /2 - uom_fontSize*2.5) ; + + //add CPU temperature + if (true) { + char val_str[16]; + char uom_str[6]; + NVGcolor val_color = nvgRGBA(255, 255, 255, 200); + if((int)(s->b.maxCpuTemp/10) > 80) { + val_color = nvgRGBA(255, 188, 3, 200); + } + if((int)(s->b.maxCpuTemp/10) > 92) { + val_color = nvgRGBA(255, 0, 0, 200); + } + // temp is alway in C * 10 + if (s->is_metric) { + snprintf(val_str, sizeof(val_str), "%d C", (int)(s->b.maxCpuTemp/10)); + } else { + snprintf(val_str, sizeof(val_str), "%d F", (int)(32+9*(s->b.maxCpuTemp/10)/5)); + } + snprintf(uom_str, sizeof(uom_str), ""); + bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "CPU TEMP", + bb_rx, bb_ry, bb_uom_dx, + val_color, lab_color, uom_color, + value_fontSize, label_fontSize, uom_fontSize ); + bb_ry = bb_y + bb_h; + } + + //add battery temperature + if (true) { + char val_str[16]; + char uom_str[6]; + NVGcolor val_color = nvgRGBA(255, 255, 255, 200); + if((int)(s->b.maxBatTemp/1000) > 40) { + val_color = nvgRGBA(255, 188, 3, 200); + } + if((int)(s->b.maxBatTemp/1000) > 50) { + val_color = nvgRGBA(255, 0, 0, 200); + } + // temp is alway in C * 1000 + if (s->is_metric) { + snprintf(val_str, sizeof(val_str), "%d C", (int)(s->b.maxBatTemp/1000)); + } else { + snprintf(val_str, sizeof(val_str), "%d F", (int)(32+9*(s->b.maxBatTemp/1000)/5)); + } + snprintf(uom_str, sizeof(uom_str), ""); + bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "BAT TEMP", + bb_rx, bb_ry, bb_uom_dx, + val_color, lab_color, uom_color, + value_fontSize, label_fontSize, uom_fontSize ); + bb_ry = bb_y + bb_h; + } + + //add grey panda GPS accuracy + if (true) { + char val_str[16]; + char uom_str[3]; + NVGcolor val_color = nvgRGBA(255, 255, 255, 200); + //show red/orange if gps accuracy is high + if(s->b.gpsAccuracy > 0.59) { + val_color = nvgRGBA(255, 188, 3, 200); + } + if(s->b.gpsAccuracy > 0.8) { + val_color = nvgRGBA(255, 0, 0, 200); + } + + + // gps accuracy is always in meters + if (s->is_metric) { + snprintf(val_str, sizeof(val_str), "%d", (int)(s->b.gpsAccuracy*100.0)); + } else { + snprintf(val_str, sizeof(val_str), "%.1f", s->b.gpsAccuracy * 3.28084 * 12); + } + if (s->is_metric) { + snprintf(uom_str, sizeof(uom_str), "cm");; + } else { + snprintf(uom_str, sizeof(uom_str), "in"); + } + bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "GPS PREC", + bb_rx, bb_ry, bb_uom_dx, + val_color, lab_color, uom_color, + value_fontSize, label_fontSize, uom_fontSize ); + bb_ry = bb_y + bb_h; + } + //add free space - from bthaler1 + if (true) { + char val_str[16]; + char uom_str[3]; + NVGcolor val_color = nvgRGBA(255, 255, 255, 200); + + //show red/orange if free space is low + if(s->b.freeSpace < 0.4) { + val_color = nvgRGBA(255, 188, 3, 200); + } + if(s->b.freeSpace < 0.2) { + val_color = nvgRGBA(255, 0, 0, 200); + } + + snprintf(val_str, sizeof(val_str), "%.1f", s->b.freeSpace* 100); + snprintf(uom_str, sizeof(uom_str), "%%"); + + bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "FREE", + bb_rx, bb_ry, bb_uom_dx, + val_color, lab_color, uom_color, + value_fontSize, label_fontSize, uom_fontSize ); + bb_ry = bb_y + bb_h; + } + //finally draw the frame + bb_h += 20; + nvgBeginPath(s->vg); + nvgRoundedRect(s->vg, bb_x, bb_y, bb_w, bb_h, 20); + nvgStrokeColor(s->vg, nvgRGBA(255,255,255,80)); + nvgStrokeWidth(s->vg, 6); + nvgStroke(s->vg); +} + + +void bb_ui_draw_measures_right( 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); + int bb_ry = bb_y; + int bb_h = 5; + NVGcolor lab_color = nvgRGBA(255, 255, 255, 200); + NVGcolor uom_color = nvgRGBA(255, 255, 255, 200); + int value_fontSize=30; + int label_fontSize=15; + int uom_fontSize = 15; + int bb_uom_dx = (int)(bb_w /2 - uom_fontSize*2.5) ; + + //add visual radar relative distance + if (true) { + char val_str[16]; + char uom_str[6]; + NVGcolor val_color = nvgRGBA(255, 255, 255, 200); + if (scene->lead_status) { + //show RED if less than 5 meters + //show orange if less than 15 meters + if((int)(scene->lead_d_rel) < 15) { + val_color = nvgRGBA(255, 188, 3, 200); + } + if((int)(scene->lead_d_rel) < 5) { + val_color = nvgRGBA(255, 0, 0, 200); + } + // lead car relative distance is always in meters + if (s->is_metric) { + snprintf(val_str, sizeof(val_str), "%d", (int)scene->lead_d_rel); + } else { + snprintf(val_str, sizeof(val_str), "%d", (int)(scene->lead_d_rel * 3.28084)); + } + } else { + snprintf(val_str, sizeof(val_str), "-"); + } + if (s->is_metric) { + snprintf(uom_str, sizeof(uom_str), "m "); + } else { + snprintf(uom_str, sizeof(uom_str), "ft"); + } + bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "REL DIST", + bb_rx, bb_ry, bb_uom_dx, + val_color, lab_color, uom_color, + value_fontSize, label_fontSize, uom_fontSize ); + bb_ry = bb_y + bb_h; + } + + //add visual radar relative speed + if (true) { + char val_str[16]; + char uom_str[6]; + NVGcolor val_color = nvgRGBA(255, 255, 255, 200); + if (scene->lead_status) { + //show Orange if negative speed (approaching) + //show Orange if negative speed faster than 5mph (approaching fast) + if((int)(scene->lead_v_rel) < 0) { + val_color = nvgRGBA(255, 188, 3, 200); + } + if((int)(scene->lead_v_rel) < -5) { + val_color = nvgRGBA(255, 0, 0, 200); + } + // lead car relative speed is always in meters + if (s->is_metric) { + snprintf(val_str, sizeof(val_str), "%d", (int)(scene->lead_v_rel * 3.6 + 0.5)); + } else { + snprintf(val_str, sizeof(val_str), "%d", (int)(scene->lead_v_rel * 2.2374144 + 0.5)); + } + } else { + snprintf(val_str, sizeof(val_str), "-"); + } + if (s->is_metric) { + snprintf(uom_str, sizeof(uom_str), "km/h");; + } else { + snprintf(uom_str, sizeof(uom_str), "mph"); + } + bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "REL SPD", + bb_rx, bb_ry, bb_uom_dx, + val_color, lab_color, uom_color, + value_fontSize, label_fontSize, uom_fontSize ); + bb_ry = bb_y + bb_h; + } + + //add steering angle + if (true) { + char val_str[16]; + char uom_str[6]; + NVGcolor val_color = nvgRGBA(255, 255, 255, 200); + //show Orange if more than 6 degrees + //show red if more than 12 degrees + if(((int)(s->b.angleSteers) < -6) || ((int)(s->b.angleSteers) > 6)) { + val_color = nvgRGBA(255, 188, 3, 200); + } + if(((int)(s->b.angleSteers) < -12) || ((int)(s->b.angleSteers) > 12)) { + val_color = nvgRGBA(255, 0, 0, 200); + } + // steering is in degrees + snprintf(val_str, sizeof(val_str), "%.1f",(s->b.angleSteers)); + + snprintf(uom_str, sizeof(uom_str), "deg"); + bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "STEER", + bb_rx, bb_ry, bb_uom_dx, + val_color, lab_color, uom_color, + value_fontSize, label_fontSize, uom_fontSize ); + bb_ry = bb_y + bb_h; + } + + //add desired steering angle + if (true) { + char val_str[16]; + char uom_str[6]; + NVGcolor val_color = nvgRGBA(255, 255, 255, 200); + //show Orange if more than 6 degrees + //show red if more than 12 degrees + if(((int)(s->b.angleSteersDes) < -6) || ((int)(s->b.angleSteersDes) > 6)) { + val_color = nvgRGBA(255, 188, 3, 200); + } + if(((int)(s->b.angleSteersDes) < -12) || ((int)(s->b.angleSteersDes) > 12)) { + val_color = nvgRGBA(255, 0, 0, 200); + } + // steering is in degrees + snprintf(val_str, sizeof(val_str), "%.1f",(s->b.angleSteersDes)); + + snprintf(uom_str, sizeof(uom_str), "deg"); + bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "DES STEER", + bb_rx, bb_ry, bb_uom_dx, + val_color, lab_color, uom_color, + value_fontSize, label_fontSize, uom_fontSize ); + bb_ry = bb_y + bb_h; + } + + + //finally draw the frame + bb_h += 20; + nvgBeginPath(s->vg); + nvgRoundedRect(s->vg, bb_x, bb_y, bb_w, bb_h, 20); + nvgStrokeColor(s->vg, nvgRGBA(255,255,255,80)); + nvgStrokeWidth(s->vg, 6); + nvgStroke(s->vg); +} + +//draw grid from wiki +void ui_draw_vision_grid( UIState *s) { + const UIScene *scene = &s->scene; + bool is_cruise_set = false;//(s->scene.v_cruise != 0 && s->scene.v_cruise != 255); + if (!is_cruise_set) { + const int grid_spacing = 30; + + int ui_viz_rx = scene->ui_viz_rx; + int ui_viz_rw = scene->ui_viz_rw; + + nvgSave(s->vg); + + // path coords are worked out in rgb-box space + nvgTranslate(s->vg, 240.0f, 0.0); + + // zooom in 2x + nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); + nvgScale(s->vg, 2.0, 2.0); + + nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height); + + nvgBeginPath(s->vg); + nvgStrokeColor(s->vg, nvgRGBA(255,255,255,128)); + nvgStrokeWidth(s->vg, 1); + + for (int i=box_y; i < box_h; i+=grid_spacing) { + nvgMoveTo(s->vg, ui_viz_rx, i); + //nvgLineTo(s->vg, ui_viz_rx, i); + nvgLineTo(s->vg, ((ui_viz_rw + ui_viz_rx) / 2)+ 10, i); + } + + for (int i=ui_viz_rx + 12; i <= ui_viz_rw; i+=grid_spacing) { + nvgMoveTo(s->vg, i, 0); + nvgLineTo(s->vg, i, 1000); + } + nvgStroke(s->vg); + nvgRestore(s->vg); + } +} + +void bb_ui_draw_logo( UIState *s) { + if ((s->status != STATUS_DISENGAGED) && (s->status != STATUS_STOPPED)) { //(s->status != STATUS_DISENGAGED) {// + return; + } + int rduration = 8000; + int logot = (bb_currentTimeInMilis() % rduration); + int logoi = s->b.img_logo; + if ((logot > (int)(rduration/4)) && (logot < (int)(3*rduration/4))) { + logoi = s->b.img_logo2; + } + if (logot < (int)(rduration/2)) { + logot = logot - (int)(rduration/4); + } else { + logot = logot - (int)(3*rduration/4); + } + float logop = fabs(4.0*logot/rduration); + const UIScene *scene = &s->scene; + const int ui_viz_rx = scene->ui_viz_rx; + const int ui_viz_rw = scene->ui_viz_rw; + const int viz_event_w = (int)(820 * logop); + const int viz_event_h = 820; + const int viz_event_x = (ui_viz_rx + (ui_viz_rw - viz_event_w - bdr_s*2)/2); + const int viz_event_y = 200; + bool is_engageable = scene->engageable; + float viz_event_alpha = 1.0f; + nvgBeginPath(s->vg); + NVGpaint imgPaint = nvgImagePattern(s->vg, viz_event_x, viz_event_y, + viz_event_w, viz_event_h, 0, logoi, viz_event_alpha); + nvgRect(s->vg, viz_event_x, viz_event_y, (int)viz_event_w, viz_event_h); + nvgFillPaint(s->vg, imgPaint); + nvgFill(s->vg); +} + +void bb_ui_read_triState_switch( UIState *s) { + //get 3-state switch position + int tri_state_fd; + char buffer[10]; + if (bb_currentTimeInMilis() - s->b.tri_state_switch_last_read > 2000) { + //tri_stated_fd = open ("/sys/devices/virtual/switch/tri-state-key/state", O_RDONLY); + //if we can't open then switch should be considered in the middle, nothing done + /* + if (tri_state_fd == -1) { + s->b.tri_state_switch = 3; + } else { + read (tri_state_fd, &buffer, 10); + s->b.tri_state_switch = buffer[0] -48; + close(tri_state_fd); + s->b.tri_state_switch_last_read = bb_currentTimeInMilis(); + } + */ + s->b.tri_state_switch_last_read = bb_currentTimeInMilis(); + s->b.tri_state_switch = 1; + if (strcmp(s->b.btns[2].btn_label2,"Left")==0) { + s->b.tri_state_switch = 1; + } + if (strcmp(s->b.btns[2].btn_label2,"Middle")==0) { + s->b.tri_state_switch = 2; + } + if (strcmp(s->b.btns[2].btn_label2,"Right")==0) { + s->b.tri_state_switch = 3; + } + + } +} + +void bb_ui_draw_UI( UIState *s) { + + + if (s->b.tri_state_switch == 1) { + const UIScene *scene = &s->scene; + const int bb_dml_w = 180; + const int bb_dml_x = (scene->ui_viz_rx + (bdr_s*2)); + const int bb_dml_y = (box_y + (bdr_s*1.5))+220; + + 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); + //bb_ui_draw_logo(s); + } + if (s->b.tri_state_switch ==2) { + const UIScene *scene = &s->scene; + const int bb_dml_w = 180; + const int bb_dml_x = (scene->ui_viz_rx + (bdr_s*2)); + const int bb_dml_y = (box_y + (bdr_s*1.5))+220; + + 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_draw_buttons(s); + bb_ui_draw_custom_alert(s); + //bb_ui_draw_logo(s); + //bb_ui_draw_car(s); + } + if (s->b.tri_state_switch ==3) { + //we now use the state 3 for minimalistic data alerts + const UIScene *scene = &s->scene; + const int bb_dml_w = 180; + const int bb_dml_x = (scene->ui_viz_rx + (bdr_s*2)); + const int bb_dml_y = (box_y + (bdr_s*1.5))+220; + + 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); + //bb_ui_draw_logo(s); + } +} + + + +void bb_ui_init(UIState *s) { + + //BB INIT + s->b.shouldDrawFrame = true; + s->status = STATUS_DISENGAGED; + strcpy(s->b.car_model,"Tesla"); + strcpy(s->b.car_folder,"tesla"); + s->b.tri_state_switch = -1; + s->b.tri_state_switch_last_read = 0; + s->b.touch_last = false; + s->b.touch_last_x = 0; + s->b.touch_last_y =0; + s->b.touch_last_width = s->scene.ui_viz_rw; + + //BB Define CAPNP sock + s->b.uiButtonInfo_sock = zsock_new_sub(">tcp://127.0.0.1:8201", ""); + assert(s->b.uiButtonInfo_sock); + s->b.uiButtonInfo_sock_raw = zsock_resolve(s->b.uiButtonInfo_sock); + + s->b.uiCustomAlert_sock = zsock_new_sub(">tcp://127.0.0.1:8202", ""); + assert(s->b.uiCustomAlert_sock); + s->b.uiCustomAlert_sock_raw = zsock_resolve(s->b.uiCustomAlert_sock); + + s->b.uiSetCar_sock = zsock_new_sub(">tcp://127.0.0.1:8203", ""); + assert(s->b.uiSetCar_sock); + s->b.uiSetCar_sock_raw = zsock_resolve(s->b.uiSetCar_sock); + + s->b.uiPlaySound_sock = zsock_new_sub(">tcp://127.0.0.1:8205", ""); + assert(s->b.uiPlaySound_sock); + s->b.uiPlaySound_sock_raw = zsock_resolve(s->b.uiPlaySound_sock); + + s->b.uiButtonStatus_sock = zsock_new_pub("@tcp://127.0.0.1:8204"); + assert(s->b.uiButtonStatus_sock); + s->b.uiButtonStatus_sock_raw = zsock_resolve(s->b.uiButtonStatus_sock); + + s->b.gps_sock = zsock_new_sub(">tcp://127.0.0.1:8032",""); + assert(s->b.gps_sock); + s->b.gps_sock_raw = zsock_resolve(s->b.gps_sock); + + //BB Load Images + s->b.img_logo = nvgCreateImage(s->vg, "../assets/img_spinner_comma.png", 1); + s->b.img_logo2 = nvgCreateImage(s->vg, "../assets/img_spinner_comma2.png", 1); + s->b.img_car = nvgCreateImage(s->vg, "../assets/img_car_tesla.png", 1); +} + +void bb_ui_play_sound( UIState *s, int sound) { + char* snd_command; + int bts = bb_get_button_status(s,"sound"); + if ((bts > 0) || (bts == -1)) { + asprintf(&snd_command, "python /data/openpilot/selfdrive/car/modules/snd/playsound.py %d &", sound); + system(snd_command); + } +} + +void bb_ui_set_car( UIState *s, char *model, char *folder) { + strcpy(s->b.car_model,model); + strcpy(s->b.car_folder, folder); +} + +void bb_ui_poll_update( UIState *s) { + int err; + zmq_pollitem_t bb_polls[8] = {{0}}; + bb_polls[0].socket = s->b.uiButtonInfo_sock_raw; + bb_polls[0].events = ZMQ_POLLIN; + bb_polls[1].socket = s->b.uiCustomAlert_sock_raw; + bb_polls[1].events = ZMQ_POLLIN; + bb_polls[2].socket = s->b.uiSetCar_sock_raw; + bb_polls[2].events = ZMQ_POLLIN; + bb_polls[3].socket = s->b.uiPlaySound_sock_raw; + bb_polls[3].events = ZMQ_POLLIN; + bb_polls[4].socket = s->b.gps_sock_raw; + bb_polls[4].events = ZMQ_POLLIN; + + //check tri-state switch + bb_ui_read_triState_switch(s); + + while (true) { + + + int ret = zmq_poll(bb_polls, 5, 0); + if (ret < 0) { + LOGW("bb poll failed (%d)", ret); + break; + } + if (ret == 0) { + //LOGW("poll empty"); + break; + } + + if (bb_polls[0].revents) { + //button info socket + zmq_msg_t msg; + err = zmq_msg_init(&msg); + assert(err == 0); + err = zmq_msg_recv(&msg, s->b.uiButtonInfo_sock_raw, 0); + assert(err >= 0); + + struct capn ctx; + capn_init_mem(&ctx, zmq_msg_data(&msg), zmq_msg_size(&msg), 0); + + cereal_UIButtonInfo_ptr stp; + stp.p = capn_getp(capn_root(&ctx), 0, 1); + struct cereal_UIButtonInfo datad; + cereal_read_UIButtonInfo(&datad, stp); + + int id = datad.btnId; + //LOGW("got button info: ID = (%d)", id); + strcpy(s->b.btns[id].btn_name,(char *)datad.btnName.str); + strcpy(s->b.btns[id].btn_label, (char *)datad.btnLabel.str); + strcpy(s->b.btns[id].btn_label2, (char *)datad.btnLabel2.str); + s->b.btns_status[id] = datad.btnStatus; + + capn_free(&ctx); + zmq_msg_close(&msg); + } + if (bb_polls[1].revents) { + //custom alert socket + zmq_msg_t msg; + err = zmq_msg_init(&msg); + assert(err == 0); + err = zmq_msg_recv(&msg, s->b.uiCustomAlert_sock_raw, 0); + assert(err >= 0); + + struct capn ctx; + capn_init_mem(&ctx, zmq_msg_data(&msg), zmq_msg_size(&msg), 0); + + cereal_UICustomAlert_ptr stp; + stp.p = capn_getp(capn_root(&ctx), 0, 1); + struct cereal_UICustomAlert datad; + cereal_read_UICustomAlert(&datad, stp); + + strcpy(s->b.custom_message,datad.caText.str); + s->b.custom_message_status = datad.caStatus; + + if ((strlen(s->b.custom_message) > 0) && (strlen(s->scene.alert_text1)==0)){ + if (!((bb_get_button_status(s,"msg") == 0) && (s->b.custom_message_status<=3))) { + set_awake(s, true); + } + } + + if ((strlen(s->scene.alert_text1) > 0) || (strlen(s->scene.alert_text2) > 0) ) { + set_awake(s, true); + } + + capn_free(&ctx); + zmq_msg_close(&msg); + // wakeup bg thread since status changed + pthread_cond_signal(&s->bg_cond); + } + if (bb_polls[2].revents) { + //set car model socket + zmq_msg_t msg; + err = zmq_msg_init(&msg); + assert(err == 0); + err = zmq_msg_recv(&msg, s->b.uiSetCar_sock_raw, 0); + assert(err >= 0); + + struct capn ctx; + capn_init_mem(&ctx, zmq_msg_data(&msg), zmq_msg_size(&msg), 0); + + cereal_UISetCar_ptr stp; + stp.p = capn_getp(capn_root(&ctx), 0, 1); + struct cereal_UISetCar datad; + cereal_read_UISetCar(&datad, stp); + + if ((strcmp(s->b.car_model,(char *) datad.icCarName.str) != 0) || (strcmp(s->b.car_folder, (char *) datad.icCarFolder.str) !=0)) { + strcpy(s->b.car_model, (char *) datad.icCarName.str); + strcpy(s->b.car_folder, (char *) datad.icCarFolder.str); + LOGW("Car folder set (%s)", s->b.car_folder); + + if (strcmp(s->b.car_folder,"tesla")==0) { + s->b.img_logo = nvgCreateImage(s->vg, "../assets/img_spinner_comma.png", 1); + s->b.img_logo2 = nvgCreateImage(s->vg, "../assets/img_spinner_comma2.png", 1); + LOGW("Spinning logo set for Tesla"); + } else if (strcmp(s->b.car_folder,"honda")==0) { + s->b.img_logo = nvgCreateImage(s->vg, "../assets/img_spinner_comma.honda.png", 1); + s->b.img_logo2 = nvgCreateImage(s->vg, "../assets/img_spinner_comma.honda2.png", 1); + LOGW("Spinning logo set for Honda"); + } else if (strcmp(s->b.car_folder,"toyota")==0) { + s->b.img_logo = nvgCreateImage(s->vg, "../assets/img_spinner_comma.toyota.png", 1); + s->b.img_logo2 = nvgCreateImage(s->vg, "../assets/img_spinner_comma.toyota2.png", 1); + LOGW("Spinning logo set for Toyota"); + }; + } + capn_free(&ctx); + zmq_msg_close(&msg); + } + if (bb_polls[3].revents) { + //play sound socket + zmq_msg_t msg; + err = zmq_msg_init(&msg); + assert(err == 0); + err = zmq_msg_recv(&msg, s->b.uiPlaySound_sock_raw, 0); + assert(err >= 0); + + struct capn ctx; + capn_init_mem(&ctx, zmq_msg_data(&msg), zmq_msg_size(&msg), 0); + + cereal_UIPlaySound_ptr stp; + stp.p = capn_getp(capn_root(&ctx), 0, 1); + struct cereal_UIPlaySound datad; + cereal_read_UIPlaySound(&datad, stp); + + int snd = datad.sndSound; + bb_ui_play_sound(s,snd); + + capn_free(&ctx); + zmq_msg_close(&msg); + } + if (bb_polls[4].revents) { + // gps socket + zmq_msg_t msg; + err = zmq_msg_init(&msg); + assert(err == 0); + err = zmq_msg_recv(&msg, s->b.gps_sock_raw, 0); + assert(err >= 0); + + struct capn ctx; + capn_init_mem(&ctx, zmq_msg_data(&msg), zmq_msg_size(&msg), 0); + + cereal_Event_ptr eventp; + eventp.p = capn_getp(capn_root(&ctx), 0, 1); + struct cereal_Event eventd; + cereal_read_Event(&eventd, eventp); + + struct cereal_GpsLocationData datad; + cereal_read_GpsLocationData(&datad, eventd.gpsLocation); + + s->b.gpsAccuracy = datad.accuracy; + if (s->b.gpsAccuracy>100) + { + s->b.gpsAccuracy=99.99; + } + else if (s->b.gpsAccuracy==0) + { + s->b.gpsAccuracy=99.8; + } + capn_free(&ctx); + zmq_msg_close(&msg); + } + + } +} diff --git a/selfdrive/ui/bbuistate.h b/selfdrive/ui/bbuistate.h new file mode 100644 index 00000000000000..718d3e2eb84fc4 --- /dev/null +++ b/selfdrive/ui/bbuistate.h @@ -0,0 +1,48 @@ +const int touch_timeout = 25; + +typedef struct UICstmButton { + char btn_name[6]; + char btn_label[6]; + char btn_label2[11]; +} UICstmButton; + +typedef struct BBUIState { + int touch_last_x; + int touch_last_y; + bool touch_last; + int touch_timeout; + int touch_last_width; + bool shouldDrawFrame; + UICstmButton btns[6]; + char btns_status[6]; + char car_model[40]; + char car_folder[20]; + zsock_t *uiButtonInfo_sock; + void *uiButtonInfo_sock_raw; + zsock_t *uiCustomAlert_sock; + void *uiCustomAlert_sock_raw; + zsock_t *uiSetCar_sock; + void *uiSetCar_sock_raw; + zsock_t *uiPlaySound_sock; + void *uiPlaySound_sock_raw; + zsock_t *uiButtonStatus_sock; + void *uiButtonStatus_sock_raw; + zsock_t *gps_sock; + void *gps_sock_raw; + int btns_x[6]; + int btns_y[6]; + int btns_r[6]; + int custom_message_status; + char custom_message[120]; + int img_logo; + int img_logo2; + int img_car; + int tri_state_switch; + long tri_state_switch_last_read; + uint16_t maxCpuTemp; + uint32_t maxBatTemp; + float gpsAccuracy ; + float freeSpace; + float angleSteers; + float angleSteersDes; +} BBUIState; diff --git a/selfdrive/ui/dashcam.h b/selfdrive/ui/dashcam.h new file mode 100644 index 00000000000000..0e9c40d77dd934 --- /dev/null +++ b/selfdrive/ui/dashcam.h @@ -0,0 +1,310 @@ +#include + +#define CAPTURE_STATE_NONE 0 +#define CAPTURE_STATE_CAPTURING 1 +#define CAPTURE_STATE_NOT_CAPTURING 2 +#define RECORD_INTERVAL 180 // Time in seconds to rotate recordings +#define RECORD_FILES 20 // Number of files to create before looping over + +typedef struct dashcam_element { + int pos_x; + int pos_y; + int width; + int height; +} dashcam_element; + +dashcam_element lock_button; + +int captureState = CAPTURE_STATE_NOT_CAPTURING; +int captureNum = 0; +int start_time = 0; +int elapsed_time = 0; // Time of current recording +char filenames[RECORD_FILES][50]; // Track the filenames so they can be deleted when rotating + +bool lock_current_video = true; // If true save the current video before rotating +bool locked_files[RECORD_FILES]; // Track which files are locked +int lock_image; // Stores reference to the PNG +int files_created = 0; + +void stop_capture() { + if (captureState == CAPTURE_STATE_CAPTURING) { + //printf("Stop capturing screen\n"); + system("killall -SIGINT screenrecord"); + captureState = CAPTURE_STATE_NOT_CAPTURING; + captureNum++; + + if (captureNum > RECORD_FILES-1) { + captureNum = 0; + } + } +} + +int get_time() { + // Get current time (in seconds) + + int iRet; + struct timeval tv; + int seconds = 0; + + iRet = gettimeofday(&tv,NULL); + if (iRet == 0) { + seconds = (int)tv.tv_sec; + } + return seconds; +} + +struct tm get_time_struct() { + time_t t = time(NULL); + struct tm tm = *localtime(&t); + return tm; +} + +void remove_file(char *videos_dir, char *filename) { + if (filename[0] == '\0') { + // Don't do anything if no filename is passed + return; + } + + int status; + char fullpath[64]; + snprintf(fullpath,sizeof(fullpath),"%s/%s", videos_dir, filename); + status = remove(fullpath); + if (status == 0) { + printf("Removed file: %s\n", fullpath); + } + else { + printf("Unable to remove file: %s\n", fullpath); + perror("Error message:"); + } +} + +void save_file(char *videos_dir, char *filename) { + if (!strlen(filename)) { + return; + } + + // Rename file to save it from being overwritten + char cmd[128]; + snprintf(cmd,sizeof(cmd), "mv %s/%s %s/saved_%s", videos_dir, filename, videos_dir, filename); + printf("save: %s\n",cmd); + system(cmd); +} + +void start_capture() { + captureState = CAPTURE_STATE_CAPTURING; + char cmd[50] = ""; + char videos_dir[50] = "/sdcard/videos"; + + ////////////////////////////////// + // NOTE: make sure videos_dir folder exists on the device! + ////////////////////////////////// + struct stat st = {0}; + if (stat(videos_dir, &st) == -1) { + mkdir(videos_dir,0700); + } + + if (strlen(filenames[captureNum]) && files_created >= RECORD_FILES) { + if (locked_files[captureNum] > 0) { + save_file(videos_dir, filenames[captureNum]); + } + else { + // remove the old file + remove_file(videos_dir, filenames[captureNum]); + } + locked_files[captureNum] = 0; + } + + char filename[64]; + struct tm tm = get_time_struct(); + snprintf(filename,sizeof(filename),"%04d%02d%02d-%02d%02d%02d.mp4", tm.tm_year + 1900, tm.tm_mon + 1, tm.tm_mday, tm.tm_hour, tm.tm_min, tm.tm_sec); + snprintf(cmd,sizeof(cmd),"screenrecord %s/%s&",videos_dir,filename); + strcpy(filenames[captureNum],filename); + + printf("Capturing to file: %s\n",cmd); + start_time = get_time(); + system(cmd); + + if (lock_current_video) { + // Lock is still on so mark this file for saving + locked_files[captureNum] = 1; + } + else { + locked_files[captureNum] = 0; + } + + files_created++; +} + +bool screen_lock_button_clicked(int touch_x, int touch_y, dashcam_element el) { + if (captureState == CAPTURE_STATE_NOT_CAPTURING) { + // Don't register click if we're not recording + return false; + } + + if (touch_x >= el.pos_x && touch_x <= el.pos_x + el.width) { + if (touch_y >= el.pos_y && touch_y <= el.pos_y + el.height) { + return true; + } + } + return false; +} + +bool screen_button_clicked(int touch_x, int touch_y) { + if (touch_x >= 1660 && touch_x <= 1810) { + if (touch_y >= 885 && touch_y <= 1035) { + return true; + } + } + return false; +} + +void draw_date_time(UIState *s) { + if (captureState == CAPTURE_STATE_NOT_CAPTURING) { + // Don't draw if we're not recording + return; + } + + // Draw the current date/time + + int rect_w = 465; + int rect_h = 80; + int rect_x = (1920-rect_w)/2; + int rect_y = (1080-rect_h-10); + + // Get local time to display + char now[50]; + struct tm tm = get_time_struct(); + snprintf(now,sizeof(now),"%04d/%02d/%02d %02d:%02d:%02d", tm.tm_year + 1900, tm.tm_mon + 1, tm.tm_mday, tm.tm_hour, tm.tm_min, tm.tm_sec); + + nvgBeginPath(s->vg); + nvgRoundedRect(s->vg, rect_x, rect_y, rect_w, rect_h, 15); + nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 100)); + nvgFill(s->vg); + nvgStrokeColor(s->vg, nvgRGBA(255,255,255,80)); + nvgStrokeWidth(s->vg, 6); + nvgStroke(s->vg); + + nvgFontSize(s->vg, 60); + nvgFontFace(s->vg, "sans-semibold"); + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200)); + nvgText(s->vg,rect_x+231,rect_y+55,now,NULL); +} + +static void rotate_video() { + // Overwrite the existing video (if needed) + elapsed_time = 0; + stop_capture(); + captureState = CAPTURE_STATE_CAPTURING; + start_capture(); +} + +void draw_lock_button(UIState *s) { + int btn_w = 150; + int btn_h = 150; + int btn_x = 1920 - btn_w - 150; + int btn_y = 1080 - btn_h; + int imgw, imgh; + + float alpha = 0.3f; + + if (!lock_image) { + // Load the lock icon + lock_image = nvgCreateImage(s->vg, "../assets/lock_icon.png", 1); + } + + if (lock_current_video) { + alpha = 1.0f; + } + + nvgBeginPath(s->vg); + NVGpaint imgPaint = nvgImagePattern(s->vg, btn_x-125, btn_y-45, 150, 150, 0, lock_image, alpha); + nvgRoundedRect(s->vg, btn_x-125, btn_y-45, 150, 150, 100); + nvgFillPaint(s->vg, imgPaint); + nvgFill(s->vg); + + + lock_button = (dashcam_element){ + .pos_x = 1500, + .pos_y = 920, + .width = 150, + .height = 150 + }; +} + +static void screen_draw_button(UIState *s, int touch_x, int touch_y) { + // Set button to bottom left of screen + if (s->vision_connected && s->plus_state == 0) { + + if (captureState == CAPTURE_STATE_CAPTURING) { + draw_lock_button(s); + } + + int btn_w = 150; + int btn_h = 150; + int btn_x = 1920 - btn_w; + int btn_y = 1080 - btn_h; + nvgBeginPath(s->vg); + nvgRoundedRect(s->vg, btn_x-110, btn_y-45, btn_w, btn_h, 100); + nvgStrokeColor(s->vg, nvgRGBA(255,255,255,80)); + nvgStrokeWidth(s->vg, 6); + nvgStroke(s->vg); + + nvgFontSize(s->vg, 70); + + if (captureState == CAPTURE_STATE_CAPTURING) { + NVGcolor fillColor = nvgRGBA(255,0,0,150); + nvgFillColor(s->vg, fillColor); + nvgFill(s->vg); + nvgFillColor(s->vg, nvgRGBA(255,255,255,200)); + } + else { + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200)); + } + nvgText(s->vg,btn_x-88,btn_y+50,"REC",NULL); + } + + if (captureState == CAPTURE_STATE_CAPTURING) { + //draw_date_time(s); + + elapsed_time = get_time() - start_time; + + if (elapsed_time >= RECORD_INTERVAL) { + rotate_video(); + } + } +} + +void screen_toggle_record_state() { + if (captureState == CAPTURE_STATE_CAPTURING) { + stop_capture(); + lock_current_video = false; + } + else { + //captureState = CAPTURE_STATE_CAPTURING; + start_capture(); + } +} + +void screen_toggle_lock() { + if (lock_current_video) { + lock_current_video = false; + } + else { + lock_current_video = true; + locked_files[captureNum] = 1; + } +} + +void dashcam( UIState *s, int touch_x, int touch_y ) { + screen_draw_button(s, touch_x, touch_y); + if (screen_button_clicked(touch_x,touch_y)) { + screen_toggle_record_state(); + } + if (screen_lock_button_clicked(touch_x,touch_y,lock_button)) { + screen_toggle_lock(); + } + if (!s->vision_connected) { + // Assume car is not in drive so stop recording + stop_capture(); + } +} \ No newline at end of file diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index dddf15f7e7167e..f3d23827f9385d 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -35,6 +35,11 @@ #include "cereal/gen/c/log.capnp.h" #include "slplay.h" +//BB include BBUIState def +#include "bbuistate.h" +//BB end + + #define STATUS_STOPPED 0 #define STATUS_DISENGAGED 1 #define STATUS_ENGAGED 2 @@ -146,6 +151,7 @@ typedef struct UIScene { uint64_t started_ts; + // Used to show gps planner status bool gps_planner_active; @@ -153,6 +159,9 @@ typedef struct UIScene { } UIScene; typedef struct UIState { + //BB define BBUIState + BBUIState b; + //BB end pthread_mutex_t lock; pthread_cond_t bg_cond; @@ -243,6 +252,8 @@ typedef struct UIState { float light_sensor; } UIState; + + static int last_brightness = -1; static void set_brightness(UIState *s, int brightness) { if (last_brightness != brightness && (s->awake || brightness == 0)) { @@ -258,13 +269,18 @@ static void set_brightness(UIState *s, int brightness) { static void set_awake(UIState *s, bool awake) { if (awake) { // 30 second timeout at 30 fps - s->awake_timeout = 30*30; + if (s->b.tri_state_switch < 3) { + s->awake_timeout = 30*30; + } else { + s->awake_timeout = 3*30; + } } if (s->awake != awake) { s->awake = awake; if (awake) { LOG("awake normal"); + set_brightness(s, 150); framebuffer_set_power(s->fb, HWC_POWER_MODE_NORMAL); } else { LOG("awake off"); @@ -274,6 +290,9 @@ static void set_awake(UIState *s, bool awake) { } } +#include "dashcam.h" +#include "bbui.h" + static void set_volume(UIState *s, int volume) { char volume_change_cmd[64]; sprintf(volume_change_cmd, "service call audio 3 i32 3 i32 %d i32 1", volume); @@ -647,28 +666,37 @@ static void draw_chevron(UIState *s, float x_in, float y_in, float sz, nvgBeginPath(s->vg); float g_xo = sz/5; float g_yo = sz/10; - if (x >= 0 && y >= 0.) { - nvgMoveTo(s->vg, x+(sz*1.35)+g_xo, y+sz+g_yo); - nvgLineTo(s->vg, x, y-g_xo); - nvgLineTo(s->vg, x-(sz*1.35)-g_xo, y+sz+g_yo); - nvgLineTo(s->vg, x+(sz*1.35)+g_xo, y+sz+g_yo); - nvgClosePath(s->vg); - } - nvgFillColor(s->vg, glowColor); - nvgFill(s->vg); - - // chevron - nvgBeginPath(s->vg); - if (x >= 0 && y >= 0.) { - nvgMoveTo(s->vg, x+(sz*1.25), y+sz); - nvgLineTo(s->vg, x, y); - nvgLineTo(s->vg, x-(sz*1.25), y+sz); - nvgLineTo(s->vg, x+(sz*1.25), y+sz); - nvgClosePath(s->vg); + //BB added for printing the car + //if position is 3 do nothing + if (s->b.tri_state_switch == 3) { + return; } - nvgFillColor(s->vg, fillColor); - nvgFill(s->vg); + if (s->b.tri_state_switch == 2) { + nvgRestore(s->vg); + bb_ui_draw_car(s); + } else { + if (x >= 0 && y >= 0.) { + nvgMoveTo(s->vg, x+(sz*1.35)+g_xo, y+sz+g_yo); + nvgLineTo(s->vg, x, y-g_xo); + nvgLineTo(s->vg, x-(sz*1.35)-g_xo, y+sz+g_yo); + nvgLineTo(s->vg, x+(sz*1.35)+g_xo, y+sz+g_yo); + nvgClosePath(s->vg); + } + nvgFillColor(s->vg, glowColor); + nvgFill(s->vg); + // chevron + nvgBeginPath(s->vg); + if (x >= 0 && y >= 0.) { + nvgMoveTo(s->vg, x+(sz*1.25), y+sz); + nvgLineTo(s->vg, x, y); + nvgLineTo(s->vg, x-(sz*1.25), y+sz); + nvgLineTo(s->vg, x+(sz*1.25), y+sz); + nvgClosePath(s->vg); + } + nvgFillColor(s->vg, fillColor); + nvgFill(s->vg); + } nvgRestore(s->vg); } @@ -722,6 +750,11 @@ static void ui_draw_lane_line(UIState *s, const float *points, float off, } static void ui_draw_lane(UIState *s, const PathData path, NVGcolor color) { + //BB added to make the line blue + if (s->b.tri_state_switch >= 2) { + color = nvgRGBA(66, 220, 244,250); + } + //BB end ui_draw_lane_line(s, path.points, 0.025*path.prob, color, false); float var = min(path.std, 0.7); color.a /= 4; @@ -861,11 +894,15 @@ static void draw_frame(UIState *s) { }; glActiveTexture(GL_TEXTURE0); - if (s->scene.frontview && s->cur_vision_front_idx >= 0) { - glBindTexture(GL_TEXTURE_2D, s->frame_front_texs[s->cur_vision_front_idx]); - } else if (!scene->frontview && s->cur_vision_idx >= 0) { - glBindTexture(GL_TEXTURE_2D, s->frame_texs[s->cur_vision_idx]); + //BB added to suppress video printing + if (s->b.tri_state_switch == 1) { + if (s->scene.frontview && s->cur_vision_front_idx >= 0) { + glBindTexture(GL_TEXTURE_2D, s->frame_front_texs[s->cur_vision_front_idx]); + } else if (!scene->frontview && s->cur_vision_idx >= 0) { + glBindTexture(GL_TEXTURE_2D, s->frame_texs[s->cur_vision_idx]); + } } + //BB end glUseProgram(s->frame_program); @@ -886,6 +923,14 @@ static void draw_frame(UIState *s) { static void ui_draw_vision_lanes(UIState *s) { const UIScene *scene = &s->scene; + //draw nothing if position is 3 + if (s->b.tri_state_switch == 3) { + return; + } + //BB add to draw our lanes + if (s->b.tri_state_switch == 2) { + bb_draw_lane_fill(s); + } // Draw left lane edge ui_draw_lane( s, scene->model.left_lane, @@ -961,8 +1006,13 @@ static void ui_draw_vision_maxspeed(UIState *s) { int viz_maxspeed_x = (ui_viz_rx + (bdr_s*2)); int viz_maxspeed_y = (box_y + (bdr_s*1.5)); int viz_maxspeed_xo = 180; + if (s->b.tri_state_switch != 2) { + viz_maxspeed_xo = 0; + } viz_maxspeed_w += viz_maxspeed_xo; + if (s->b.tri_state_switch == 2) { viz_maxspeed_x += viz_maxspeed_w - (viz_maxspeed_xo * 2); + } // Draw Background nvgBeginPath(s->vg); @@ -972,11 +1022,14 @@ static void ui_draw_vision_maxspeed(UIState *s) { } else { nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 100)); } - nvgFill(s->vg); + if (s->b.tri_state_switch != 2) { + nvgFill(s->vg); + } // Draw Border nvgBeginPath(s->vg); nvgRoundedRect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, 20); + if (s->b.tri_state_switch != 2) { if (is_set_over_limit) { nvgStrokeColor(s->vg, nvgRGBA(218, 111, 37, 255)); } else if (is_speedlim_valid && !s->is_ego_over_limit) { @@ -988,7 +1041,7 @@ static void ui_draw_vision_maxspeed(UIState *s) { } nvgStrokeWidth(s->vg, 10); nvgStroke(s->vg); - + } // Draw "MAX" Text nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); nvgFontFace(s->vg, "sans-regular"); @@ -1013,6 +1066,9 @@ static void ui_draw_vision_maxspeed(UIState *s) { nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 100)); nvgText(s->vg, viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 242, "N/A", NULL); } + //BB START: add new measures panel const int bb_dml_w = 180; + bb_ui_draw_UI(s) ; + //BB END: add new measures panel } static void ui_draw_vision_speedlimit(UIState *s) { @@ -1215,7 +1271,9 @@ static void ui_draw_vision_header(UIState *s) { nvgFill(s->vg); ui_draw_vision_maxspeed(s); - ui_draw_vision_speedlimit(s); + if (s->b.tri_state_switch == 2) { + ui_draw_vision_speedlimit(s); + } ui_draw_vision_speed(s); ui_draw_vision_wheel(s); } @@ -1284,9 +1342,19 @@ static void ui_draw_vision_alert(UIState *s, int va_size, int va_color, nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM); nvgTextBox(s->vg, alr_x, alr_h-(longAlert1?300:360), alr_w-60, va_text2, NULL); } + } + static void ui_draw_vision(UIState *s) { + //BB code added to only draw every other frame + if (!s->b.shouldDrawFrame) { + s->b.shouldDrawFrame = true; + //return; + } + s->b.shouldDrawFrame = false; + //BBEND + const UIScene *scene = &s->scene; int ui_viz_rx = scene->ui_viz_rx; int ui_viz_rw = scene->ui_viz_rw; @@ -1401,10 +1469,17 @@ static ModelData read_model(cereal_ModelData_ptr modelp) { } static void update_status(UIState *s, int status) { + //BB Variable for the old status + int old_status = s->status; if (s->status != status) { s->status = status; + set_awake(s, true); // wake up bg thread to change pthread_cond_signal(&s->bg_cond); + //BB add sound + if ((old_status != STATUS_STOPPED) || (s->status != STATUS_DISENGAGED)) { + bb_ui_play_sound(s,s->status); + } } } @@ -1518,8 +1593,10 @@ static void ui_update(UIState *s) { if (polls[0].revents || polls[1].revents || polls[2].revents || polls[3].revents || polls[4].revents || polls[6].revents || polls[7].revents || polls[8].revents) { - // awake on any (old) activity - set_awake(s, true); + // awake on any (old) activity if tri-state in 1 or 2 position + if(s->b.tri_state_switch < 3) { + set_awake(s, true); + } } if (s->vision_connected && polls[9].revents) { @@ -1616,6 +1693,10 @@ static void ui_update(UIState *s) { } s->scene.v_cruise = datad.vCruise; s->scene.v_ego = datad.vEgo; + //BB get angles + s->b.angleSteers = datad.angleSteers; + s->b.angleSteersDes = datad.angleSteersDes; + //BB END s->scene.curvature = datad.curvature; s->scene.engaged = datad.enabled; s->scene.engageable = datad.engageable; @@ -1721,6 +1802,7 @@ static void ui_update(UIState *s) { struct cereal_LiveCalibrationData datad; cereal_read_LiveCalibrationData(&datad, eventd.liveCalibration); + // should we still even have this? capn_list32 warpl = datad.warpMatrix2; capn_resolve(&warpl.p); // is this a bug? @@ -1766,23 +1848,40 @@ static void ui_update(UIState *s) { } s->scene.started_ts = datad.startedTs; - } else if (eventd.which == cereal_Event_uiLayoutState) { - struct cereal_UiLayoutState datad; - cereal_read_UiLayoutState(&datad, eventd.uiLayoutState); - s->scene.uilayout_sidebarcollapsed = datad.sidebarCollapsed; - s->scene.uilayout_mapenabled = datad.mapEnabled; - - bool hasSidebar = !s->scene.uilayout_sidebarcollapsed; - bool mapEnabled = s->scene.uilayout_mapenabled; - if (mapEnabled) { - s->scene.ui_viz_rx = hasSidebar ? (box_x+nav_w) : (box_x+nav_w-(bdr_s*4)); - s->scene.ui_viz_rw = hasSidebar ? (box_w-nav_w) : (box_w-nav_w+(bdr_s*4)); - s->scene.ui_viz_ro = -(sbr_w + 4*bdr_s); - } else { - s->scene.ui_viz_rx = hasSidebar ? box_x : (box_x-sbr_w+bdr_s*2); - s->scene.ui_viz_rw = hasSidebar ? box_w : (box_w+sbr_w-(bdr_s*2)); - s->scene.ui_viz_ro = hasSidebar ? -(sbr_w - 6*bdr_s) : 0; + //BB CPU TEMP + s->b.maxCpuTemp=datad.cpu0; + if (s->b.maxCpuTempb.maxCpuTemp=datad.cpu1; + } + else if (s->b.maxCpuTempb.maxCpuTemp=datad.cpu2; } + else if (s->b.maxCpuTempb.maxCpuTemp=datad.cpu3; + } + s->b.maxBatTemp=datad.bat; + s->b.freeSpace=datad.freeSpace; + //BB END CPU TEMP + } else if (eventd.which == cereal_Event_uiLayoutState) { + struct cereal_UiLayoutState datad; + cereal_read_UiLayoutState(&datad, eventd.uiLayoutState); + s->scene.uilayout_sidebarcollapsed = datad.sidebarCollapsed; + s->scene.uilayout_mapenabled = datad.mapEnabled; + + bool hasSidebar = !s->scene.uilayout_sidebarcollapsed; + bool mapEnabled = s->scene.uilayout_mapenabled; + if (mapEnabled) { + s->scene.ui_viz_rx = hasSidebar ? (box_x+nav_w) : (box_x+nav_w-(bdr_s*4)); + s->scene.ui_viz_rw = hasSidebar ? (box_w-nav_w) : (box_w-nav_w+(bdr_s*4)); + s->scene.ui_viz_ro = -(sbr_w + 4*bdr_s); + } else { + s->scene.ui_viz_rx = hasSidebar ? box_x : (box_x-sbr_w+bdr_s*2); + s->scene.ui_viz_rw = hasSidebar ? box_w : (box_w+sbr_w-(bdr_s*2)); + s->scene.ui_viz_ro = hasSidebar ? -(sbr_w - 6*bdr_s) : 0; + } } else if (eventd.which == cereal_Event_liveMapData) { struct cereal_LiveMapData datad; cereal_read_LiveMapData(&datad, eventd.liveMapData); @@ -1922,11 +2021,14 @@ static void* bg_thread(void* args) { int bg_status = -1; while(!do_exit) { pthread_mutex_lock(&s->lock); - if (bg_status == s->status) { + //BB Change of background based on our color + int actual_status = bb_get_status(s); + if (bg_status == actual_status) { // will always be signaled if it changes? pthread_cond_wait(&s->bg_cond, &s->lock); } - bg_status = s->status; + bg_status = actual_status; + //BB End of background color change pthread_mutex_unlock(&s->lock); assert(bg_status < ARRAYSIZE(bg_colors)); @@ -1968,6 +2070,8 @@ int main() { UIState uistate; UIState *s = &uistate; ui_init(s); + //BB init our UI + bb_ui_init(s); pthread_t connect_thread_handle; err = pthread_create(&connect_thread_handle, NULL, @@ -2026,11 +2130,34 @@ int main() { // awake on any touch int touch_x = -1, touch_y = -1; - int touched = touch_poll(&touch, &touch_x, &touch_y, s->awake ? 0 : 100); + int touched = touch_poll(&touch, &touch_x, &touch_y, s->awake ? 20 : 500); + int dc_touch_x = -1, dc_touch_y = -1; + s->b.touch_timeout = max(s->b.touch_timeout -1,0); + if (touched == 1) { // touch event will still happen :( set_awake(s, true); - } + s->b.touch_last = true; + s->b.touch_last_x = touch_x; + s->b.touch_last_y = touch_y; + s->b.touch_timeout = touch_timeout; + s->b.touch_last_width = s->scene.ui_viz_rw; + } + //BB check touch + if ((s->b.touch_last) && (s->b.touch_last_width != s->scene.ui_viz_rw)) { + bb_handle_ui_touch(s,s->b.touch_last_x,s->b.touch_last_y); + dc_touch_x = s->b.touch_last_x; + dc_touch_y = s->b.touch_last_y; + s->b.touch_last = false; + s->b.touch_last_x = 0; + s->b.touch_last_y = 0; + s->b.touch_last_width=s->scene.ui_viz_rw; + } + + //s->b.touch_last_width = s->scene.ui_viz_rw; + //BB Update our cereal polls + bb_ui_poll_update(s); + // manage wakefulness if (s->awake_timeout > 0) { @@ -2040,6 +2167,7 @@ int main() { } if (s->awake) { + dashcam(s, dc_touch_x, dc_touch_y); ui_draw(s); glFinish(); should_swap = true; diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond old mode 100755 new mode 100644 index caf5de38ae68a7..b26fbdeda38674 Binary files a/selfdrive/visiond/visiond and b/selfdrive/visiond/visiond differ diff --git a/selfdrive/visiond/visiond-devel b/selfdrive/visiond/visiond-devel new file mode 100755 index 00000000000000..40259afc55c789 Binary files /dev/null and b/selfdrive/visiond/visiond-devel differ diff --git a/selfdrive/visiond/visiond-normal b/selfdrive/visiond/visiond-normal new file mode 100755 index 00000000000000..caf5de38ae68a7 Binary files /dev/null and b/selfdrive/visiond/visiond-normal differ diff --git a/selfdrive/visiond/visiond-wiggly b/selfdrive/visiond/visiond-wiggly new file mode 100644 index 00000000000000..b26fbdeda38674 Binary files /dev/null and b/selfdrive/visiond/visiond-wiggly differ