From aee4f9719c9d1e8d10e937d6511cebf23f6e7298 Mon Sep 17 00:00:00 2001 From: Rick Lan Date: Sun, 22 Nov 2020 16:38:23 +1000 Subject: [PATCH] https://github.com/commaai/openpilot/pull/2590 https://github.com/commaai/openpilot/pull/2578 --- launch_chffrplus.sh | 13 +++++++++++++ selfdrive/loggerd/loggerd.cc | 26 ++++++++++---------------- selfdrive/modeld/modeld.cc | 17 +++++++++++------ selfdrive/modeld/models/driving.cc | 20 +++++++++++++------- selfdrive/modeld/models/driving.h | 9 ++++++--- selfdrive/rtshield.py | 7 +++++-- selfdrive/thermald/thermald.py | 2 -- 7 files changed, 58 insertions(+), 36 deletions(-) diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 0e41ea359..9dd01cd97 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -19,6 +19,19 @@ function two_init { # openpilot gets all the cores echo 0-3 > /dev/cpuset/app/cpus + # set up governors + # +50mW offroad, +500mW onroad for 30% more RAM bandwidth + echo "performance" > /sys/class/devfreq/soc:qcom,cpubw/governor + echo 1056000 > /sys/class/devfreq/soc:qcom,m4m/max_freq + echo "performance" > /sys/class/devfreq/soc:qcom,m4m/governor + + # unclear if these help, but they don't seem to hurt + echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu0/governor + echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu2/governor + + # /sys/class/devfreq/soc:qcom,mincpubw is the only one left at "powersave" + # it seems to gain nothing but a wasted 500mW + # Collect RIL and other possibly long-running I/O interrupts onto CPU 1 echo 1 > /proc/irq/78/smp_affinity_list # qcom,smd-modem (LTE radio) echo 1 > /proc/irq/33/smp_affinity_list # ufshcd (flash storage) diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index 3a9e64e46..6360a555d 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -67,7 +67,6 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = { .stream_type = VISION_STREAM_YUV, .filename = "fcamera.hevc", .frame_packet_name = "frame", - .encode_idx_name = "encodeIdx", .fps = MAIN_FPS, .bitrate = MAIN_BITRATE, .is_h265 = true, @@ -78,7 +77,6 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = { .stream_type = VISION_STREAM_YUV_FRONT, .filename = "dcamera.hevc", .frame_packet_name = "frontFrame", - .encode_idx_name = "frontEncodeIdx", .fps = MAIN_FPS, // on EONs, more compressed this way .bitrate = DCAM_BITRATE, .is_h265 = true, @@ -89,7 +87,6 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = { .stream_type = VISION_STREAM_YUV_WIDE, .filename = "ecamera.hevc", .frame_packet_name = "wideFrame", - .encode_idx_name = "wideEncodeIdx", .fps = MAIN_FPS, .bitrate = MAIN_BITRATE, .is_h265 = true, @@ -235,9 +232,6 @@ void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) { s.num_encoder += 1; pthread_mutex_unlock(&s.rotate_lock); - PubSocket *idx_sock = PubSocket::create(s.ctx, cameras_logged[cam_idx].encode_idx_name); - assert(idx_sock != NULL); - LoggerHandle *lh = NULL; while (!do_exit) { @@ -377,8 +371,12 @@ void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) { // publish encode index MessageBuilder msg; - auto eidx = msg.initEvent().initEncodeIdx(); + // this is really ugly + auto eidx = cam_idx == LOG_CAMERA_ID_DCAMERA ? msg.initEvent().initFrontEncodeIdx() : + (cam_idx == LOG_CAMERA_ID_ECAMERA ? msg.initEvent().initWideEncodeIdx() : msg.initEvent().initEncodeIdx()); eidx.setFrameId(extra.frame_id); + eidx.setTimestampSof(extra.timestamp_sof); + eidx.setTimestampEof(extra.timestamp_eof); #ifdef QCOM2 eidx.setType(cereal::EncodeIndex::Type::FULL_H_E_V_C); #else @@ -390,10 +388,6 @@ void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) { eidx.setSegmentId(out_id); auto bytes = msg.toBytes(); - - if (idx_sock->send((char*)bytes.begin(), bytes.size()) < 0) { - printf("err sending encodeIdx pkt: %s\n", strerror(errno)); - } if (lh) { lh_log(lh, bytes.begin(), bytes.size(), false); } @@ -453,8 +447,6 @@ void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) { visionstream_destroy(&stream); } - delete idx_sock; - if (encoder_inited) { LOG("encoder destroy"); encoder_close(&encoder); @@ -606,6 +598,10 @@ static void bootlog() { int main(int argc, char** argv) { int err; +#ifdef QCOM + setpriority(PRIO_PROCESS, 0, -12); +#endif + if (argc > 1 && strcmp(argv[1], "--bootlog") == 0) { bootlog(); return 0; @@ -620,8 +616,6 @@ int main(int argc, char** argv) { record_front = Params().read_db_bool("RecordFront"); #endif - setpriority(PRIO_PROCESS, 0, -12); - clear_locks(); signal(SIGINT, (sighandler_t)set_do_exit); @@ -751,7 +745,7 @@ int main(int argc, char** argv) { if (s.logger.part > -1) { new_segment = true; - if (tms - last_camera_seen_tms <= NO_CAMERA_PATIENCE) { + if (tms - last_camera_seen_tms <= NO_CAMERA_PATIENCE && s.num_encoder > 0) { for (int cid=0;cid<=MAX_CAM_IDX;cid++) { // this *should* be redundant on tici since all camera frames are synced new_segment &= (((s.rotate_state[cid].stream_frame_id >= s.rotate_state[cid].last_rotate_frame_id + segment_length * MAIN_FPS) && diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 7250fd38d..86319d5b6 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -147,7 +147,7 @@ int main(int argc, char **argv) { // setup filter to track dropped frames const float dt = 1. / MODEL_FREQ; - const float ts = 5.0; // 5 s filter time constant + const float ts = 10.0; // filter time constant (s) const float frame_filter_k = (dt / ts) / (1. + dt / ts); float frames_dropped = 0; @@ -157,6 +157,7 @@ int main(int argc, char **argv) { uint32_t frame_id = 0, last_vipc_frame_id = 0; double last = 0; int desire = -1; + uint32_t run_count = 0; while (!do_exit) { VIPCBuf *buf; VIPCBufExtra extra; @@ -179,6 +180,8 @@ int main(int argc, char **argv) { double mt1 = 0, mt2 = 0; if (run_model_this_iter) { + run_count++; + float vec_desire[DESIRE_LEN] = {0}; if (desire >= 0 && desire < DESIRE_LEN) { vec_desire[desire] = 1.0; @@ -193,17 +196,19 @@ int main(int argc, char **argv) { model_eval_frame(&model, q, yuv_ion.buf_cl, buf_info.width, buf_info.height, model_transform, NULL, vec_desire); mt2 = millis_since_boot(); + float model_execution_time = (mt2 - mt1) / 1000.0; // tracked dropped frames uint32_t vipc_dropped_frames = extra.frame_id - last_vipc_frame_id - 1; frames_dropped = (1. - frame_filter_k) * frames_dropped + frame_filter_k * (float)std::min(vipc_dropped_frames, 10U); - float frame_drop_perc = frames_dropped / MODEL_FREQ; + if (run_count < 10) frames_dropped = 0; // let frame drops warm up + float frame_drop_ratio = frames_dropped / (1 + frames_dropped); - model_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_perc, model_buf, extra.timestamp_eof); - model_publish_v2(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_perc, model_buf, extra.timestamp_eof); - posenet_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_perc, model_buf, extra.timestamp_eof); + model_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time); + model_publish_v2(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time); + posenet_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof); - LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_perc); + LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_ratio); last = mt1; last_vipc_frame_id = extra.frame_id; } diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 301e7fafb..585c8bb65 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -307,7 +307,8 @@ void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data, void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, uint32_t vipc_dropped_frames, float frame_drop, - const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { + const ModelDataRaw &net_outputs, uint64_t timestamp_eof, + float model_execution_time) { // make msg MessageBuilder msg; auto framed = msg.initEvent(frame_drop < MAX_FRAME_DROP).initModelV2(); @@ -316,8 +317,9 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, framed.setFrameAge(frame_age); framed.setFrameDropPerc(frame_drop * 100); framed.setTimestampEof(timestamp_eof); + framed.setModelExecutionTime(model_execution_time); - // plan + // plan int plan_mhp_max_idx = 0; for (int i=1; i @@ -366,7 +368,7 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, // meta auto meta = framed.initMeta(); fill_meta_v2(meta, net_outputs.meta); - + // leads auto leads = framed.initLeads(LEAD_MHP_SELECTION); int mdn_max_idx = 0; @@ -385,15 +387,18 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, } void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { - uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; + uint32_t vipc_dropped_frames, float frame_drop, + const ModelDataRaw &net_outputs, uint64_t timestamp_eof, + float model_execution_time) { + uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; MessageBuilder msg; auto framed = msg.initEvent(frame_drop < MAX_FRAME_DROP).initModel(); framed.setFrameId(vipc_frame_id); framed.setFrameAge(frame_age); framed.setFrameDropPerc(frame_drop * 100); framed.setTimestampEof(timestamp_eof); + framed.setModelExecutionTime(model_execution_time); // Find the distribution that corresponds to the most probable plan int plan_mhp_max_idx = 0; @@ -424,7 +429,7 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, auto lpath = framed.initPath(); fill_path(lpath, &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)], valid_len, valid_len_idx); - + auto left_lane = framed.initLeftLane(); int ll_idx = 1; fill_lane_line(left_lane, net_outputs.lane_lines, ll_idx, valid_len, valid_len_idx, @@ -461,7 +466,8 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, } void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { + uint32_t vipc_dropped_frames, float frame_drop, + const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { float trans_arr[3]; float trans_std_arr[3]; float rot_arr[3]; diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 613ba139c..346e6e876 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -78,9 +78,12 @@ void model_free(ModelState* s); void poly_fit(float *in_pts, float *in_stds, float *out); void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, uint64_t timestamp_eof); + uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, + uint64_t timestamp_eof, float model_execution_time); void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, uint64_t timestamp_eof); + uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, + uint64_t timestamp_eof, float model_execution_time); void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, uint64_t timestamp_eof); + uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, + uint64_t timestamp_eof); #endif diff --git a/selfdrive/rtshield.py b/selfdrive/rtshield.py index e76022501..2e7bd3c6a 100644 --- a/selfdrive/rtshield.py +++ b/selfdrive/rtshield.py @@ -1,16 +1,19 @@ #!/usr/bin/env python3 +import os import time from common.realtime import set_core_affinity, set_realtime_priority - # RT shield - ensure CPU 3 always remains available for RT processes # runs as SCHED_FIFO with minimum priority to ensure kthreads don't # get scheduled onto CPU 3, but it's always preemptible by realtime # openpilot processes def main(): - set_core_affinity(3) + set_core_affinity(int(os.getenv("CORE", "3"))) set_realtime_priority(1) while True: time.sleep(0.000001) + +if __name__ == "__main__": + main() diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index a206e5657..cd6803ddb 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -410,7 +410,6 @@ def thermald_thread(): if started_ts is None: started_ts = sec_since_boot() started_seen = True - os.system('echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor') else: if startup_conditions["ignition"] and (startup_conditions != startup_conditions_prev): cloudlog.event("Startup blocked", startup_conditions=startup_conditions) @@ -420,7 +419,6 @@ def thermald_thread(): started_ts = None if off_ts is None: off_ts = sec_since_boot() - os.system('echo powersave > /sys/class/devfreq/soc:qcom,cpubw/governor') # Offroad power monitoring pm.calculate(health)