mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-08-22 16:53:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

https://github.com/commaai/openpilot/pull/2590

Browse Source 
https://github.com/commaai/openpilot/pull/2578
This commit is contained in:
Rick Lan
2020-11-22 16:38:23 +10:00
parent 4c34b3cae7
commit aee4f9719c
7 changed files with 58 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch_chffrplus.sh
+13
View File
@@ -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)
selfdrive/loggerd/loggerd.cc
+10 -16
View File
@@ -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) &&
selfdrive/modeld/modeld.cc
+11 -6
View File
@@ -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;
}
selfdrive/modeld/models/driving.cc
+13 -7
View File
@@ -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<PLAN_MHP_N; i++) {
if (net_outputs.plan[(i + 1)*(PLAN_MHP_GROUP_SIZE) - 1] >
@@ -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];
selfdrive/modeld/models/driving.h
+6 -3
View File
@@ -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
selfdrive/rtshield.py
+5 -2
View File
@@ -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()
selfdrive/thermald/thermald.py
-2
View File
@@ -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)