From e0b7cce478debd22baaaa53ed082916f3b8a6ef0 Mon Sep 17 00:00:00 2001 From: Rick Lan Date: Tue, 24 Nov 2020 13:17:41 +1000 Subject: [PATCH] 2020-11-21, 2020-11-23, 2020-11-24 changes in master-ci --- cereal/car.capnp | 2 ++ launch_chffrplus.sh | 12 +++++++- selfdrive/car/fw_versions.py | 22 +++++++------- selfdrive/car/toyota/values.py | 2 ++ selfdrive/controls/controlsd.py | 9 ++++-- selfdrive/controls/lib/events.py | 8 ++++++ selfdrive/modeld/SConscript | 6 +++- selfdrive/modeld/dmonitoringmodeld.cc | 5 ++-- selfdrive/modeld/models/dmonitoring.cc | 40 +++++++++++++++++++------- selfdrive/modeld/runners/snpemodel.cc | 1 + selfdrive/modeld/thneed/thneed.cc | 11 ++++++- selfdrive/modeld/thneed/thneed.h | 5 ++++ 12 files changed, 95 insertions(+), 28 deletions(-) diff --git a/cereal/car.capnp b/cereal/car.capnp index 03f8324e0..78deec3bf 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -102,6 +102,8 @@ struct CarEvent @0x9b1657f34caf3ad3 { modeldLagging @89; deviceFalling @90; fanMalfunction @91; + cameraMalfunction @92; + modelLagWarning @93; gasUnavailableDEPRECATED @3; dataNeededDEPRECATED @16; diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 9dd01cd97..08baac73b 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -8,6 +8,11 @@ source "$BASEDIR/launch_env.sh" DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" +function tici_init { + sudo su -c 'echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu0/governor' + sudo su -c 'echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu4/governor' +} + function two_init { # Restrict Android and other system processes to the first two cores echo 0-1 > /dev/cpuset/background/cpus @@ -36,11 +41,12 @@ function two_init { echo 1 > /proc/irq/78/smp_affinity_list # qcom,smd-modem (LTE radio) echo 1 > /proc/irq/33/smp_affinity_list # ufshcd (flash storage) echo 1 > /proc/irq/35/smp_affinity_list # wifi (wlan_pci) + echo 1 > /proc/irq/6/smp_affinity_list # MDSS + # USB traffic needs realtime handling on cpu 3 [ -d "/proc/irq/733" ] && echo 3 > /proc/irq/733/smp_affinity_list # USB for LeEco [ -d "/proc/irq/736" ] && echo 3 > /proc/irq/736/smp_affinity_list # USB for OP3T - # Check for NEOS update if [ $(< /VERSION) != "$REQUIRED_NEOS_VERSION" ]; then if [ -f "$DIR/scripts/continue.sh" ]; then @@ -122,6 +128,10 @@ function launch { two_init fi + if [ -f /TICI ]; then + tici_init + fi + # handle pythonpath ln -sfn $(pwd) /data/pythonpath export PYTHONPATH="$PWD" diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index ca5346052..c85b7fa43 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -28,7 +28,7 @@ SHORT_TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40]) DEFAULT_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, uds.SESSION_TYPE.DEFAULT]) DEFAULT_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, - uds.SESSION_TYPE.DEFAULT, 0x0, 0x32, 0x1, 0xf4]) + uds.SESSION_TYPE.DEFAULT, 0x0, 0x32, 0x1, 0xf4]) EXTENDED_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC]) @@ -36,20 +36,20 @@ EXTENDED_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTRO uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC, 0x0, 0x32, 0x1, 0xf4]) UDS_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) UDS_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) HYUNDAI_VERSION_REQUEST_SHORT = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(0xf1a0) # 4 Byte version number + p16(0xf1a0) # 4 Byte version number HYUNDAI_VERSION_REQUEST_LONG = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(0xf100) # Long description + p16(0xf100) # Long description HYUNDAI_VERSION_REQUEST_MULTI = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \ - p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + \ - p16(0xf100) + \ - p16(0xf1a0) + p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \ + p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + \ + p16(0xf100) + \ + p16(0xf1a0) HYUNDAI_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) @@ -127,8 +127,8 @@ def match_fw_to_car(fw_versions): if ecu_type == Ecu.esp and candidate in [TOYOTA.RAV4, TOYOTA.COROLLA, TOYOTA.HIGHLANDER] and found_version is None: continue - # TODO: COROLLA_TSS2 engine can show on two different addresses - if ecu_type == Ecu.engine and candidate in [TOYOTA.COROLLA_TSS2, TOYOTA.CHR] and found_version is None: + # TODO: on some toyota, the engine can show on two different addresses + if ecu_type == Ecu.engine and candidate in [TOYOTA.COROLLA_TSS2, TOYOTA.CHR, TOYOTA.LEXUS_IS] and found_version is None: continue # ignore non essential ecus diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index f78252276..75c1c62f3 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -649,6 +649,7 @@ FW_VERSIONS = { b'\x03312M3000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ + b'\x018965B1255000\x00\x00\x00\x00', b'8965B12361\x00\x00\x00\x00\x00\x00', b'\x018965B12350\x00\x00\x00\x00\x00\x00', b'\x018965B12470\x00\x00\x00\x00\x00\x00', @@ -1030,6 +1031,7 @@ FW_VERSIONS = { b'\x01896634A19100\x00\x00\x00\x00', b'\x01896634A20000\x00\x00\x00\x00', b'\x01896634A22000\x00\x00\x00\x00', + b'\x01896634A45000\x00\x00\x00\x00', b'\x01896634A46000\x00\x00\x00\x00', b'\x01F152642551\x00\x00\x00\x00\x00\x00', b'\x028966342T0000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 556ff2f16..538cee125 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -247,8 +247,13 @@ class Controls: self.events.add(EventName.relayMalfunction) if self.sm['plan'].fcw: self.events.add(EventName.fcw) - if self.sm['model'].frameDropPerc > 1 and (not SIMULATION): - self.events.add(EventName.modeldLagging) + if not self.sm.alive['frontFrame'] and (self.sm.frame > 5 / DT_CTRL) and not SIMULATION: + self.events.add(EventName.cameraMalfunction) + + if self.sm['model'].frameDropPerc > 20 and not SIMULATION: + self.events.add(EventName.modeldLagging) + elif self.sm['model'].frameDropPerc > 2 and not SIMULATION: + self.events.add(EventName.modelLagWarning) # Only allow engagement with brake pressed when stopped behind another stopped car if not self.sm['dragonConf'].dpAtl and CS.brakePressed and self.sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED \ diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index d13b3d9e7..eddecf9b0 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -481,6 +481,14 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimePrompt, 1., 1., 1.), }, + EventName.modelLagWarning: { + ET.WARNING: Alert( + _("TAKE CONTROL"), + _("Driving Model Lagging"), + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimePrompt, 1., 1., 1.), + }, + EventName.fanMalfunction: { ET.PERMANENT: NormalPermanentAlert(_("Fan Malfunction"), _("Contact Support")), }, diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 4409033c5..569bcf0e7 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -19,7 +19,11 @@ if arch == "aarch64": lenv['CFLAGS'].append("-DUSE_THNEED") lenv['CXXFLAGS'].append("-DUSE_THNEED") elif arch == "larch64": - libs += ['gsl', 'CB', 'pthread'] + libs += ['gsl', 'CB', 'pthread', 'dl'] + if not TEST_THNEED: + common_src += ["thneed/thneed.cc"] + lenv['CFLAGS'].append("-DUSE_THNEED") + lenv['CXXFLAGS'].append("-DUSE_THNEED") else: libs += ['pthread'] diff --git a/selfdrive/modeld/dmonitoringmodeld.cc b/selfdrive/modeld/dmonitoringmodeld.cc index 4eeb292ef..3a9b134ef 100644 --- a/selfdrive/modeld/dmonitoringmodeld.cc +++ b/selfdrive/modeld/dmonitoringmodeld.cc @@ -3,6 +3,7 @@ #include #include #include +#include #include "common/visionbuf.h" #include "common/visionipc.h" @@ -23,7 +24,7 @@ static void set_do_exit(int sig) { int main(int argc, char **argv) { int err; - set_realtime_priority(51); + setpriority(PRIO_PROCESS, 0, -15); #ifdef QCOM2 set_core_affinity(5); @@ -65,7 +66,7 @@ int main(int argc, char **argv) { double t2 = millis_since_boot(); // send dm packet - dmonitoring_publish(pm, extra.frame_id, res); + dmonitoring_publish(pm, extra.frame_id, res, (t2-t1)/1000.0); LOGD("dmonitoring process: %.2fms, from last %.2fms", t2-t1, t1-last); last = t1; diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index 6be44c926..fd262801e 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -114,24 +114,39 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ resized_width, resized_height, mode); + // prerotate to be cache aware + uint8_t *resized_buf_rot = get_buffer(s->resized_buf_rot, resized_width*resized_height*3/2); + uint8_t *resized_y_buf_rot = resized_buf_rot; + uint8_t *resized_u_buf_rot = resized_y_buf_rot + (resized_width * resized_height); + uint8_t *resized_v_buf_rot = resized_u_buf_rot + ((resized_width/2) * (resized_height/2)); + + libyuv::I420Rotate(resized_y_buf, resized_width, + resized_u_buf, resized_width/2, + resized_v_buf, resized_width/2, + resized_y_buf_rot, resized_height, + resized_u_buf_rot, resized_height/2, + resized_v_buf_rot, resized_height/2, + // negative height causes a vertical flip to match previous + resized_width, -resized_height, libyuv::kRotate90); + int yuv_buf_len = (MODEL_WIDTH/2) * (MODEL_HEIGHT/2) * 6; // Y|u|v -> y|y|y|y|u|v float *net_input_buf = get_buffer(s->net_input_buf, yuv_buf_len); // one shot conversion, O(n) anyway // yuvframe2tensor, normalize - for (int r = 0; r < MODEL_HEIGHT/2; r++) { - for (int c = 0; c < MODEL_WIDTH/2; c++) { + for (int c = 0; c < MODEL_WIDTH/2; c++) { + for (int r = 0; r < MODEL_HEIGHT/2; r++) { // Y_ul - net_input_buf[(c*MODEL_HEIGHT/2) + r] = input_lambda(resized_buf[(2*r*resized_width) + (2*c)]); - // Y_ur - net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width) + (2*c+1)]); + net_input_buf[(c*MODEL_HEIGHT/2) + r + (0*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r) + (2*c)*resized_height]); // Y_dl - net_input_buf[(c*MODEL_HEIGHT/2) + r + ((MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c)]); + net_input_buf[(c*MODEL_HEIGHT/2) + r + (1*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r+1) + (2*c)*resized_height]); + // Y_ur + net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r) + (2*c+1)*resized_height]); // Y_dr - net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c+1)]); + net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r+1) + (2*c+1)*resized_height]); // U - net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + (r*resized_width/2) + c]); + net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(resized_width*resized_height) + r + (c*resized_height/2)]); // V - net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + (r*resized_width/2) + c]); + net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + r + (c*resized_height/2)]); } } @@ -140,6 +155,10 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ //fwrite(raw_buf, height*width*3/2, sizeof(uint8_t), dump_yuv_file); //fclose(dump_yuv_file); + // *** testing *** + // idat = np.frombuffer(open("/tmp/inputdump.yuv", "rb").read(), np.float32).reshape(6, 160, 320) + // imshow(cv2.cvtColor(tensor_to_frames(idat[None]/0.0078125+128)[0], cv2.COLOR_YUV2RGB_I420)) + //FILE *dump_yuv_file2 = fopen("/tmp/inputdump.yuv", "wb"); //fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file2); //fclose(dump_yuv_file2); @@ -165,11 +184,12 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ return ret; } -void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res){ +void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time){ // make msg MessageBuilder msg; auto framed = msg.initEvent().initDriverState(); framed.setFrameId(frame_id); + framed.setModelExecutionTime(execution_time); kj::ArrayPtr face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation)); kj::ArrayPtr face_orientation_std(&res.face_orientation_meta[0], ARRAYSIZE(res.face_orientation_meta)); diff --git a/selfdrive/modeld/runners/snpemodel.cc b/selfdrive/modeld/runners/snpemodel.cc index 38da13453..2197c24cd 100644 --- a/selfdrive/modeld/runners/snpemodel.cc +++ b/selfdrive/modeld/runners/snpemodel.cc @@ -1,6 +1,7 @@ #pragma clang diagnostic ignored "-Wexceptions" #include +#include #include #include "common/util.h" #include "snpemodel.h" diff --git a/selfdrive/modeld/thneed/thneed.cc b/selfdrive/modeld/thneed/thneed.cc index 11f10cfeb..4c1adc1ab 100644 --- a/selfdrive/modeld/thneed/thneed.cc +++ b/selfdrive/modeld/thneed/thneed.cc @@ -1,10 +1,11 @@ -#include "thneed.h" #include #include #include #include #include +#include #include +#include "thneed.h" Thneed *g_thneed = NULL; int g_fd = -1; @@ -31,6 +32,7 @@ extern "C" { int (*my_ioctl)(int filedes, unsigned long request, void *argp) = NULL; #undef ioctl int ioctl(int filedes, unsigned long request, void *argp) { + request &= 0xFFFFFFFF; // needed on QCOM2 if (my_ioctl == NULL) my_ioctl = reinterpret_cast(dlsym(RTLD_NEXT, "ioctl")); Thneed *thneed = g_thneed; @@ -440,7 +442,14 @@ cl_program thneed_clCreateProgramWithSource(cl_context context, cl_uint count, c #endif void *dlsym(void *handle, const char *symbol) { + // TODO: Find dlsym in a better way. Currently this is hand looked up in libdl.so +#if defined QCOM void *(*my_dlsym)(void *handle, const char *symbol) = (void *(*)(void *handle, const char *symbol))((uintptr_t)dlopen-0x2d4); +#elif defined QCOM2 + void *(*my_dlsym)(void *handle, const char *symbol) = (void *(*)(void *handle, const char *symbol))((uintptr_t)dlopen+0x138); +#else + #error "Unsupported platform for thneed" +#endif if (memcmp("REAL_", symbol, 5) == 0) { return my_dlsym(handle, symbol+5); } else if (strcmp("clEnqueueNDRangeKernel", symbol) == 0) { diff --git a/selfdrive/modeld/thneed/thneed.h b/selfdrive/modeld/thneed/thneed.h index 36f0bfed7..07e54e134 100644 --- a/selfdrive/modeld/thneed/thneed.h +++ b/selfdrive/modeld/thneed/thneed.h @@ -1,5 +1,10 @@ #pragma once +#ifndef __user + #define __user __attribute__(()) +#endif + +#include #include #include "include/msm_kgsl.h" #include