diff --git a/selfdrive/modeld/dmonitoringmodeld.cc b/selfdrive/modeld/dmonitoringmodeld.cc index 48b16980bb..2c531d7cf0 100644 --- a/selfdrive/modeld/dmonitoringmodeld.cc +++ b/selfdrive/modeld/dmonitoringmodeld.cc @@ -1,10 +1,7 @@ #include #include -#include -#include #include -#include "visionbuf.h" #include "visionipc_client.h" #include "common/swaglog.h" #include "common/util.h" @@ -15,51 +12,47 @@ #include #endif - ExitHandler do_exit; -int main(int argc, char **argv) { - setpriority(PRIO_PROCESS, 0, -15); - +void run_model(DMonitoringModelState &model, VisionIpcClient &vipc_client) { PubMaster pm({"driverState"}); + double last = 0; - // init the models - DMonitoringModelState dmonitoringmodel; - dmonitoring_init(&dmonitoringmodel); + while (!do_exit) { + VisionIpcBufExtra extra = {}; + VisionBuf *buf = vipc_client.recv(&extra); + if (buf == nullptr) continue; - VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_YUV_FRONT, true); - while (!do_exit){ - if (!vipc_client.connect(false)){ - util::sleep_for(100); - continue; - } - break; - } + double t1 = millis_since_boot(); + DMonitoringResult res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height); + double t2 = millis_since_boot(); - while (!do_exit) { - LOGW("connected with buffer size: %d", vipc_client.buffers[0].len); + // send dm packet + dmonitoring_publish(pm, extra.frame_id, res, (t2 - t1) / 1000.0, model.output); - double last = 0; - while (!do_exit) { - VisionIpcBufExtra extra = {0}; - VisionBuf *buf = vipc_client.recv(&extra); - if (buf == nullptr){ - continue; - } + LOGD("dmonitoring process: %.2fms, from last %.2fms", t2 - t1, t1 - last); + last = t1; + } +} - double t1 = millis_since_boot(); - DMonitoringResult res = dmonitoring_eval_frame(&dmonitoringmodel, buf->addr, buf->width, buf->height); - double t2 = millis_since_boot(); +int main(int argc, char **argv) { + setpriority(PRIO_PROCESS, 0, -15); - // send dm packet - dmonitoring_publish(pm, extra.frame_id, res, (t2-t1)/1000.0, dmonitoringmodel.output); + // init the models + DMonitoringModelState model; + dmonitoring_init(&model); - LOGD("dmonitoring process: %.2fms, from last %.2fms", t2-t1, t1-last); - last = t1; - } + VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_YUV_FRONT, true); + while (!do_exit && !vipc_client.connect(false)) { + util::sleep_for(100); } - dmonitoring_free(&dmonitoringmodel); + // run the models + if (vipc_client.connected) { + LOGW("connected with buffer size: %d", vipc_client.buffers[0].len); + run_model(model, vipc_client); + } + dmonitoring_free(&model); return 0; } diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 83d0dec131..59f1a8e751 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -1,9 +1,8 @@ #include #include -#include +#include #include -#include "visionbuf.h" #include "visionipc_client.h" #include "common/swaglog.h" #include "common/clutil.h" @@ -14,12 +13,12 @@ ExitHandler do_exit; // globals -bool run_model; +bool live_calib_seen; mat3 cur_transform; -pthread_mutex_t transform_lock; +std::mutex transform_lock; -void* live_thread(void *arg) { - set_thread_name("live"); +void calibration_thread() { + set_thread_name("calibration"); set_realtime_priority(50); SubMaster sm({"liveCalibration"}); @@ -60,17 +59,77 @@ void* live_thread(void *arg) { transform.v[i] = warp_matrix(i / 3, i % 3); } mat3 model_transform = matmul3(yuv_transform, transform); - pthread_mutex_lock(&transform_lock); + std::lock_guard lk(transform_lock); cur_transform = model_transform; - run_model = true; - pthread_mutex_unlock(&transform_lock); + live_calib_seen = true; + } + } +} + +void run_model(ModelState &model, VisionIpcClient &vipc_client) { + // messaging + PubMaster pm({"modelV2", "cameraOdometry"}); + SubMaster sm({"lateralPlan", "roadCameraState"}); + + // setup filter to track dropped frames + const float dt = 1. / MODEL_FREQ; + const float ts = 10.0; // filter time constant (s) + const float frame_filter_k = (dt / ts) / (1. + dt / ts); + float frames_dropped = 0; + + uint32_t frame_id = 0, last_vipc_frame_id = 0; + double last = 0; + int desire = -1; + uint32_t run_count = 0; + + while (!do_exit) { + VisionIpcBufExtra extra = {}; + VisionBuf *buf = vipc_client.recv(&extra); + if (buf == nullptr) continue; + + transform_lock.lock(); + mat3 model_transform = cur_transform; + const bool run_model_this_iter = live_calib_seen; + transform_lock.unlock(); + + if (sm.update(0) > 0) { + // TODO: path planner timeout? + desire = ((int)sm["lateralPlan"].getLateralPlan().getDesire()); + frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId(); + } + + if (run_model_this_iter) { + run_count++; + + float vec_desire[DESIRE_LEN] = {0}; + if (desire >= 0 && desire < DESIRE_LEN) { + vec_desire[desire] = 1.0; + } + + double mt1 = millis_since_boot(); + ModelDataRaw model_buf = model_eval_frame(&model, buf->buf_cl, buf->width, buf->height, + model_transform, vec_desire); + double 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); + 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, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time, + kj::ArrayPtr(model.output.data(), model.output.size())); + posenet_publish(pm, extra.frame_id, vipc_dropped_frames, model_buf, extra.timestamp_eof); + + LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f", mt2 - mt1, mt1 - last, extra.frame_id, frame_id, frame_drop_ratio); + last = mt1; + last_vipc_frame_id = extra.frame_id; } } - return NULL; } int main(int argc, char **argv) { - int err; set_realtime_priority(54); #ifdef QCOM @@ -80,16 +139,8 @@ int main(int argc, char **argv) { set_core_affinity(4); #endif - pthread_mutex_init(&transform_lock, NULL); - // start calibration thread - pthread_t live_thread_handle; - err = pthread_create(&live_thread_handle, NULL, live_thread, NULL); - assert(err == 0); - - // messaging - PubMaster pm({"modelV2", "cameraOdometry"}); - SubMaster sm({"lateralPlan", "roadCameraState"}); + std::thread thread = std::thread(calibration_thread); // cl init cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); @@ -101,90 +152,21 @@ int main(int argc, char **argv) { LOGW("models loaded, modeld starting"); VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, true, device_id, context); - - while (!do_exit){ - if (!vipc_client.connect(false)){ - util::sleep_for(100); - continue; - } - break; + while (!do_exit && !vipc_client.connect(false)) { + util::sleep_for(100); } - // loop - while (!do_exit) { - VisionBuf *b = &vipc_client.buffers[0]; + // run the models + // vipc_client.connected is false only when do_exit is true + if (vipc_client.connected) { + const VisionBuf *b = &vipc_client.buffers[0]; LOGW("connected with buffer size: %d (%d x %d)", b->len, b->width, b->height); - - // setup filter to track dropped frames - const float dt = 1. / MODEL_FREQ; - const float ts = 10.0; // filter time constant (s) - const float frame_filter_k = (dt / ts) / (1. + dt / ts); - float frames_dropped = 0; - - uint32_t frame_id = 0, last_vipc_frame_id = 0; - double last = 0; - int desire = -1; - uint32_t run_count = 0; - - while (!do_exit) { - VisionIpcBufExtra extra; - VisionBuf *buf = vipc_client.recv(&extra); - if (buf == nullptr){ - continue; - } - - pthread_mutex_lock(&transform_lock); - mat3 model_transform = cur_transform; - const bool run_model_this_iter = run_model; - pthread_mutex_unlock(&transform_lock); - - if (sm.update(0) > 0){ - // TODO: path planner timeout? - desire = ((int)sm["lateralPlan"].getLateralPlan().getDesire()); - frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId(); - } - - 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; - } - - mt1 = millis_since_boot(); - - ModelDataRaw model_buf = - model_eval_frame(&model, buf->buf_cl, buf->width, buf->height, - model_transform, 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); - 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, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time, - kj::ArrayPtr(model.output.data(), model.output.size())); - posenet_publish(pm, extra.frame_id, vipc_dropped_frames, model_buf, extra.timestamp_eof); - - LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_ratio); - last = mt1; - last_vipc_frame_id = extra.frame_id; - } - - } + run_model(model, vipc_client); } model_free(&model); - - LOG("joining live_thread"); - err = pthread_join(live_thread_handle, NULL); - assert(err == 0); + LOG("joining calibration thread"); + thread.join(); CL_CHECK(clReleaseContext(context)); - pthread_mutex_destroy(&transform_lock); return 0; }