ZwX1616
3 years ago
commit
743259bbb1
255 changed files with 1839 additions and 2971 deletions
@ -0,0 +1,28 @@ |
|||||||
|
name: release |
||||||
|
on: |
||||||
|
schedule: |
||||||
|
- cron: '0 * * * *' |
||||||
|
workflow_dispatch: |
||||||
|
|
||||||
|
jobs: |
||||||
|
build_masterci: |
||||||
|
name: build master-ci |
||||||
|
runs-on: ubuntu-20.04 |
||||||
|
timeout-minutes: 60 |
||||||
|
if: github.repository == 'commaai/openpilot' |
||||||
|
steps: |
||||||
|
- name: Wait for green check mark |
||||||
|
uses: lewagon/wait-on-check-action@v0.2 |
||||||
|
with: |
||||||
|
ref: master |
||||||
|
wait-interval: 30 |
||||||
|
running-workflow-name: 'build master-ci' |
||||||
|
- uses: actions/checkout@v3 |
||||||
|
with: |
||||||
|
submodules: true |
||||||
|
fetch-depth: 0 |
||||||
|
- name: Pull LFS |
||||||
|
run: git lfs pull |
||||||
|
- name: Build master-ci |
||||||
|
run: | |
||||||
|
BRANCH=master-ci release/build_devel.sh |
||||||
@ -1 +1 @@ |
|||||||
Subproject commit 7fc82bbc06900ff723992af4d7add10dc3afc0f5 |
Subproject commit d61736e301cf27d10b74beac278214f974c55fe4 |
||||||
@ -1,4 +1,39 @@ |
|||||||
Import('envCython', 'common') |
Import('env', 'envCython', 'arch', 'SHARED') |
||||||
|
|
||||||
|
if SHARED: |
||||||
|
fxn = env.SharedLibrary |
||||||
|
else: |
||||||
|
fxn = env.Library |
||||||
|
|
||||||
|
common_libs = [ |
||||||
|
'params.cc', |
||||||
|
'statlog.cc', |
||||||
|
'swaglog.cc', |
||||||
|
'util.cc', |
||||||
|
'gpio.cc', |
||||||
|
'i2c.cc', |
||||||
|
'watchdog.cc', |
||||||
|
] |
||||||
|
|
||||||
|
_common = fxn('common', common_libs, LIBS="json11") |
||||||
|
|
||||||
|
files = [ |
||||||
|
'clutil.cc', |
||||||
|
'visionimg.cc', |
||||||
|
] |
||||||
|
|
||||||
|
if arch == "larch64": |
||||||
|
_gpu_libs = ["GLESv2"] |
||||||
|
else: |
||||||
|
_gpu_libs = ["GL"] |
||||||
|
|
||||||
|
_gpucommon = fxn('gpucommon', files, LIBS=_gpu_libs) |
||||||
|
Export('_common', '_gpucommon', '_gpu_libs') |
||||||
|
|
||||||
|
if GetOption('test'): |
||||||
|
env.Program('tests/test_util', ['tests/test_util.cc'], LIBS=[_common]) |
||||||
|
env.Program('tests/test_swaglog', ['tests/test_swaglog.cc'], LIBS=[_common, 'json11', 'zmq', 'pthread']) |
||||||
|
|
||||||
|
# Cython |
||||||
envCython.Program('clock.so', 'clock.pyx') |
envCython.Program('clock.so', 'clock.pyx') |
||||||
envCython.Program('params_pyx.so', 'params_pyx.pyx', LIBS=envCython['LIBS'] + [common, 'zmq']) |
envCython.Program('params_pyx.so', 'params_pyx.pyx', LIBS=envCython['LIBS'] + [_common, 'zmq', 'json11']) |
||||||
|
|||||||
@ -1,10 +1,10 @@ |
|||||||
#include "selfdrive/common/clutil.h" |
#include "common/clutil.h" |
||||||
|
|
||||||
#include <cassert> |
#include <cassert> |
||||||
#include <iostream> |
#include <iostream> |
||||||
#include <memory> |
#include <memory> |
||||||
|
|
||||||
#include "selfdrive/common/util.h" |
#include "common/util.h" |
||||||
|
|
||||||
namespace { // helper functions
|
namespace { // helper functions
|
||||||
|
|
||||||
@ -1,11 +1,11 @@ |
|||||||
#include "selfdrive/common/gpio.h" |
#include "common/gpio.h" |
||||||
|
|
||||||
#include <fcntl.h> |
#include <fcntl.h> |
||||||
#include <unistd.h> |
#include <unistd.h> |
||||||
|
|
||||||
#include <cstring> |
#include <cstring> |
||||||
|
|
||||||
#include "selfdrive/common/util.h" |
#include "common/util.h" |
||||||
|
|
||||||
// We assume that all pins have already been exported on boot,
|
// We assume that all pins have already been exported on boot,
|
||||||
// and that we have permission to write to them.
|
// and that we have permission to write to them.
|
||||||
@ -1,48 +0,0 @@ |
|||||||
from typing import List |
|
||||||
|
|
||||||
HTML_REPLACEMENTS = [ |
|
||||||
(r'&', r'&'), |
|
||||||
(r'"', r'"'), |
|
||||||
] |
|
||||||
|
|
||||||
|
|
||||||
def parse_markdown(text: str, tab_length: int = 2) -> str: |
|
||||||
lines = text.split("\n") |
|
||||||
output: List[str] = [] |
|
||||||
list_level = 0 |
|
||||||
|
|
||||||
def end_outstanding_lists(level: int, end_level: int) -> int: |
|
||||||
while level > end_level: |
|
||||||
level -= 1 |
|
||||||
output.append("</ul>") |
|
||||||
if level > 0: |
|
||||||
output.append("</li>") |
|
||||||
return end_level |
|
||||||
|
|
||||||
for i, line in enumerate(lines): |
|
||||||
if i + 1 < len(lines) and lines[i + 1].startswith("==="): # heading |
|
||||||
output.append(f"<h1>{line}</h1>") |
|
||||||
elif line.startswith("==="): |
|
||||||
pass |
|
||||||
elif line.lstrip().startswith("* "): # list |
|
||||||
line_level = 1 + line.count(" " * tab_length, 0, line.index("*")) |
|
||||||
if list_level >= line_level: |
|
||||||
list_level = end_outstanding_lists(list_level, line_level) |
|
||||||
else: |
|
||||||
list_level += 1 |
|
||||||
if list_level > 1: |
|
||||||
output[-1] = output[-1].replace("</li>", "") |
|
||||||
output.append("<ul>") |
|
||||||
output.append(f"<li>{line.replace('*', '', 1).lstrip()}</li>") |
|
||||||
else: |
|
||||||
list_level = end_outstanding_lists(list_level, 0) |
|
||||||
if len(line) > 0: |
|
||||||
output.append(line) |
|
||||||
|
|
||||||
end_outstanding_lists(list_level, 0) |
|
||||||
output_str = "\n".join(output) + "\n" |
|
||||||
|
|
||||||
for (fr, to) in HTML_REPLACEMENTS: |
|
||||||
output_str = output_str.replace(fr, to) |
|
||||||
|
|
||||||
return output_str |
|
||||||
@ -1,7 +1,7 @@ |
|||||||
#pragma once |
#pragma once |
||||||
|
|
||||||
#include <array> |
#include <array> |
||||||
#include "selfdrive/common/mat.h" |
#include "common/mat.h" |
||||||
#include "selfdrive/hardware/hw.h" |
#include "selfdrive/hardware/hw.h" |
||||||
|
|
||||||
const int TRAJECTORY_SIZE = 33; |
const int TRAJECTORY_SIZE = 33; |
||||||
@ -1,6 +0,0 @@ |
|||||||
def replace_right(s, old, new, occurrence): |
|
||||||
# replace_right('1232425', '2', ' ', 1) -> '12324 5' |
|
||||||
# replace_right('1232425', '2', ' ', 2) -> '123 4 5' |
|
||||||
|
|
||||||
split = s.rsplit(old, occurrence) |
|
||||||
return new.join(split) |
|
||||||
@ -1,6 +1,6 @@ |
|||||||
#pragma once |
#pragma once |
||||||
|
|
||||||
#include "selfdrive/common/timing.h" |
#include "common/timing.h" |
||||||
|
|
||||||
#define CLOUDLOG_DEBUG 10 |
#define CLOUDLOG_DEBUG 10 |
||||||
#define CLOUDLOG_INFO 20 |
#define CLOUDLOG_INFO 20 |
||||||
@ -0,0 +1,2 @@ |
|||||||
|
test_util |
||||||
|
test_swaglog |
||||||
@ -1,26 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
from markdown_it import MarkdownIt |
|
||||||
import os |
|
||||||
import unittest |
|
||||||
|
|
||||||
from common.basedir import BASEDIR |
|
||||||
from common.markdown import parse_markdown |
|
||||||
|
|
||||||
|
|
||||||
class TestMarkdown(unittest.TestCase): |
|
||||||
# validate that our simple markdown parser produces the same output as `markdown_it` from pip |
|
||||||
def test_current_release_notes(self): |
|
||||||
self.maxDiff = None |
|
||||||
|
|
||||||
with open(os.path.join(BASEDIR, "RELEASES.md")) as f: |
|
||||||
for r in f.read().split("\n\n"): |
|
||||||
|
|
||||||
# No hyperlink support is ok |
|
||||||
if '[' in r: |
|
||||||
continue |
|
||||||
|
|
||||||
self.assertEqual(MarkdownIt().render(r), parse_markdown(r)) |
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__": |
|
||||||
unittest.main() |
|
||||||
@ -1,47 +0,0 @@ |
|||||||
import os |
|
||||||
import tempfile |
|
||||||
import shutil |
|
||||||
import unittest |
|
||||||
|
|
||||||
from common.xattr import getxattr, setxattr, listxattr, removexattr |
|
||||||
|
|
||||||
class TestParams(unittest.TestCase): |
|
||||||
USER_TEST='user.test' |
|
||||||
def setUp(self): |
|
||||||
self.tmpdir = tempfile.mkdtemp() |
|
||||||
self.tmpfn = os.path.join(self.tmpdir, 'test.txt') |
|
||||||
open(self.tmpfn, 'w').close() |
|
||||||
#print("using", self.tmpfn) |
|
||||||
|
|
||||||
def tearDown(self): |
|
||||||
shutil.rmtree(self.tmpdir) |
|
||||||
|
|
||||||
def test_getxattr_none(self): |
|
||||||
a = getxattr(self.tmpfn, TestParams.USER_TEST) |
|
||||||
assert a is None |
|
||||||
|
|
||||||
def test_listxattr_none(self): |
|
||||||
l = listxattr(self.tmpfn) |
|
||||||
assert l == [] |
|
||||||
|
|
||||||
def test_setxattr(self): |
|
||||||
setxattr(self.tmpfn, TestParams.USER_TEST, b'123') |
|
||||||
a = getxattr(self.tmpfn, TestParams.USER_TEST) |
|
||||||
assert a == b'123' |
|
||||||
|
|
||||||
def test_listxattr(self): |
|
||||||
setxattr(self.tmpfn, 'user.test1', b'123') |
|
||||||
setxattr(self.tmpfn, 'user.test2', b'123') |
|
||||||
l = listxattr(self.tmpfn) |
|
||||||
assert l == ['user.test1', 'user.test2'] |
|
||||||
|
|
||||||
def test_removexattr(self): |
|
||||||
setxattr(self.tmpfn, TestParams.USER_TEST, b'123') |
|
||||||
a = getxattr(self.tmpfn, TestParams.USER_TEST) |
|
||||||
assert a == b'123' |
|
||||||
removexattr(self.tmpfn, TestParams.USER_TEST) |
|
||||||
a = getxattr(self.tmpfn, TestParams.USER_TEST) |
|
||||||
assert a is None |
|
||||||
|
|
||||||
if __name__ == "__main__": |
|
||||||
unittest.main() |
|
||||||
@ -1,4 +1,4 @@ |
|||||||
#include "selfdrive/common/util.h" |
#include "common/util.h" |
||||||
|
|
||||||
#include <sys/stat.h> |
#include <sys/stat.h> |
||||||
#include <dirent.h> |
#include <dirent.h> |
||||||
@ -1,4 +1,4 @@ |
|||||||
#include "selfdrive/common/visionimg.h" |
#include "common/visionimg.h" |
||||||
|
|
||||||
#include <cassert> |
#include <cassert> |
||||||
|
|
||||||
@ -1,6 +1,6 @@ |
|||||||
#include "selfdrive/common/watchdog.h" |
#include "common/watchdog.h" |
||||||
#include "selfdrive/common/timing.h" |
#include "common/timing.h" |
||||||
#include "selfdrive/common/util.h" |
#include "common/util.h" |
||||||
|
|
||||||
const std::string watchdog_fn_prefix = "/dev/shm/wd_"; // + <pid>
|
const std::string watchdog_fn_prefix = "/dev/shm/wd_"; // + <pid>
|
||||||
|
|
||||||
@ -1 +1 @@ |
|||||||
Subproject commit be1a213a5ffa3cafe2b4f2d53f6df5d2452ad910 |
Subproject commit f5f76d28b4827c3fb706d542729651ceef6c06bd |
||||||
@ -1 +1 @@ |
|||||||
Subproject commit 9564b74d80525c9f289b730febbb2348c529c9cc |
Subproject commit 210237fa635eeb76ad855c2031d2cad3bde3a2c0 |
||||||
@ -1 +1 @@ |
|||||||
Subproject commit d5bd81e5b517c79e164d87b96355e6bc75915da0 |
Subproject commit 36c62afa0c170aa1b7a39bcae3316ffb844499e8 |
||||||
@ -1,5 +1,9 @@ |
|||||||
selfdrive/ui/replay/* |
selfdrive/modeld/runners/onnx* |
||||||
|
|
||||||
third_party/mapbox-gl-native-qt/x86_64/** |
third_party/mapbox-gl-native-qt/x86_64/*.so |
||||||
|
|
||||||
third_party/qt-plugins/x86_64/** |
third_party/qt-plugins/x86_64/geoservices/*.so |
||||||
|
|
||||||
|
third_party/libyuv/x64/** |
||||||
|
third_party/snpe/x86_64/** |
||||||
|
third_party/snpe/x86_64-linux-clang/** |
||||||
|
|||||||
Binary file not shown.
@ -1,125 +0,0 @@ |
|||||||
#include "selfdrive/camerad/cameras/camera_replay.h" |
|
||||||
|
|
||||||
#include <cassert> |
|
||||||
#include <thread> |
|
||||||
|
|
||||||
#include "selfdrive/common/clutil.h" |
|
||||||
#include "selfdrive/common/util.h" |
|
||||||
|
|
||||||
extern ExitHandler do_exit; |
|
||||||
|
|
||||||
void camera_autoexposure(CameraState *s, float grey_frac) {} |
|
||||||
|
|
||||||
namespace { |
|
||||||
|
|
||||||
const char *BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/"; |
|
||||||
|
|
||||||
const std::string road_camera_route = "0c94aa1e1296d7c6|2021-05-05--19-48-37"; |
|
||||||
// const std::string driver_camera_route = "534ccd8a0950a00c|2021-06-08--12-15-37";
|
|
||||||
|
|
||||||
std::string get_url(std::string route_name, const std::string &camera, int segment_num) { |
|
||||||
std::replace(route_name.begin(), route_name.end(), '|', '/'); |
|
||||||
return util::string_format("%s%s/%d/%s.hevc", BASE_URL, route_name.c_str(), segment_num, camera.c_str()); |
|
||||||
} |
|
||||||
|
|
||||||
void camera_init(VisionIpcServer *v, CameraState *s, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type, const std::string &url) { |
|
||||||
s->frame = new FrameReader(); |
|
||||||
if (!s->frame->load(url)) { |
|
||||||
printf("failed to load stream from %s", url.c_str()); |
|
||||||
assert(0); |
|
||||||
} |
|
||||||
|
|
||||||
CameraInfo ci = { |
|
||||||
.frame_width = (uint32_t)s->frame->width, |
|
||||||
.frame_height = (uint32_t)s->frame->height, |
|
||||||
.frame_stride = (uint32_t)s->frame->width * 3, |
|
||||||
}; |
|
||||||
s->ci = ci; |
|
||||||
s->camera_num = camera_id; |
|
||||||
s->fps = fps; |
|
||||||
s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type); |
|
||||||
} |
|
||||||
|
|
||||||
void camera_close(CameraState *s) { |
|
||||||
delete s->frame; |
|
||||||
} |
|
||||||
|
|
||||||
void run_camera(CameraState *s) { |
|
||||||
uint32_t stream_frame_id = 0, frame_id = 0; |
|
||||||
size_t buf_idx = 0; |
|
||||||
std::unique_ptr<uint8_t[]> rgb_buf = std::make_unique<uint8_t[]>(s->frame->getRGBSize()); |
|
||||||
std::unique_ptr<uint8_t[]> yuv_buf = std::make_unique<uint8_t[]>(s->frame->getYUVSize()); |
|
||||||
while (!do_exit) { |
|
||||||
if (stream_frame_id == s->frame->getFrameCount()) { |
|
||||||
// loop stream
|
|
||||||
stream_frame_id = 0; |
|
||||||
} |
|
||||||
if (s->frame->get(stream_frame_id++, rgb_buf.get(), yuv_buf.get())) { |
|
||||||
s->buf.camera_bufs_metadata[buf_idx] = {.frame_id = frame_id}; |
|
||||||
auto &buf = s->buf.camera_bufs[buf_idx]; |
|
||||||
CL_CHECK(clEnqueueWriteBuffer(buf.copy_q, buf.buf_cl, CL_TRUE, 0, s->frame->getRGBSize(), rgb_buf.get(), 0, NULL, NULL)); |
|
||||||
s->buf.queue(buf_idx); |
|
||||||
++frame_id; |
|
||||||
buf_idx = (buf_idx + 1) % FRAME_BUF_COUNT; |
|
||||||
} |
|
||||||
util::sleep_for(1000 / s->fps); |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
void road_camera_thread(CameraState *s) { |
|
||||||
util::set_thread_name("replay_road_camera_thread"); |
|
||||||
run_camera(s); |
|
||||||
} |
|
||||||
|
|
||||||
// void driver_camera_thread(CameraState *s) {
|
|
||||||
// util::set_thread_name("replay_driver_camera_thread");
|
|
||||||
// run_camera(s);
|
|
||||||
// }
|
|
||||||
|
|
||||||
void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { |
|
||||||
const CameraBuf *b = &c->buf; |
|
||||||
MessageBuilder msg; |
|
||||||
auto framed = msg.initEvent().initRoadCameraState(); |
|
||||||
fill_frame_data(framed, b->cur_frame_data); |
|
||||||
framed.setImage(kj::arrayPtr((const uint8_t *)b->cur_yuv_buf->addr, b->cur_yuv_buf->len)); |
|
||||||
framed.setTransform(b->yuv_transform.v); |
|
||||||
s->pm->send("roadCameraState", msg); |
|
||||||
} |
|
||||||
|
|
||||||
// void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) {
|
|
||||||
// MessageBuilder msg;
|
|
||||||
// auto framed = msg.initEvent().initDriverCameraState();
|
|
||||||
// framed.setFrameType(cereal::FrameData::FrameType::FRONT);
|
|
||||||
// fill_frame_data(framed, c->buf.cur_frame_data);
|
|
||||||
// s->pm->send("driverCameraState", msg);
|
|
||||||
// }
|
|
||||||
|
|
||||||
} // namespace
|
|
||||||
|
|
||||||
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) { |
|
||||||
camera_init(v, &s->road_cam, CAMERA_ID_LGC920, 20, device_id, ctx, |
|
||||||
VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD, get_url(road_camera_route, "fcamera", 0)); |
|
||||||
// camera_init(v, &s->driver_cam, CAMERA_ID_LGC615, 10, device_id, ctx,
|
|
||||||
// VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER, get_url(driver_camera_route, "dcamera", 0));
|
|
||||||
s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"}); |
|
||||||
} |
|
||||||
|
|
||||||
void cameras_open(MultiCameraState *s) {} |
|
||||||
|
|
||||||
void cameras_close(MultiCameraState *s) { |
|
||||||
camera_close(&s->road_cam); |
|
||||||
camera_close(&s->driver_cam); |
|
||||||
delete s->pm; |
|
||||||
} |
|
||||||
|
|
||||||
void cameras_run(MultiCameraState *s) { |
|
||||||
std::vector<std::thread> threads; |
|
||||||
threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera)); |
|
||||||
// threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_camera));
|
|
||||||
// threads.push_back(std::thread(driver_camera_thread, &s->driver_cam));
|
|
||||||
road_camera_thread(&s->road_cam); |
|
||||||
|
|
||||||
for (auto &t : threads) t.join(); |
|
||||||
|
|
||||||
cameras_close(s); |
|
||||||
} |
|
||||||
@ -1,25 +0,0 @@ |
|||||||
#pragma once |
|
||||||
|
|
||||||
#include "selfdrive/camerad/cameras/camera_common.h" |
|
||||||
#include "selfdrive/ui/replay/framereader.h" |
|
||||||
|
|
||||||
#define FRAME_BUF_COUNT 16 |
|
||||||
|
|
||||||
typedef struct CameraState { |
|
||||||
int camera_num; |
|
||||||
CameraInfo ci; |
|
||||||
|
|
||||||
int fps; |
|
||||||
float digital_gain = 0; |
|
||||||
|
|
||||||
CameraBuf buf; |
|
||||||
FrameReader *frame = nullptr; |
|
||||||
} CameraState; |
|
||||||
|
|
||||||
typedef struct MultiCameraState { |
|
||||||
CameraState road_cam; |
|
||||||
CameraState driver_cam; |
|
||||||
|
|
||||||
SubMaster *sm = nullptr; |
|
||||||
PubMaster *pm = nullptr; |
|
||||||
} MultiCameraState; |
|
||||||
@ -1,197 +0,0 @@ |
|||||||
#include "selfdrive/camerad/cameras/camera_webcam.h" |
|
||||||
|
|
||||||
#include <unistd.h> |
|
||||||
|
|
||||||
#include <cassert> |
|
||||||
#include <cstring> |
|
||||||
|
|
||||||
#pragma clang diagnostic push |
|
||||||
#pragma clang diagnostic ignored "-Wundefined-inline" |
|
||||||
#include <opencv2/core.hpp> |
|
||||||
#include <opencv2/highgui.hpp> |
|
||||||
#include <opencv2/opencv.hpp> |
|
||||||
#include <opencv2/videoio.hpp> |
|
||||||
#pragma clang diagnostic pop |
|
||||||
|
|
||||||
#include "selfdrive/common/clutil.h" |
|
||||||
#include "selfdrive/common/swaglog.h" |
|
||||||
#include "selfdrive/common/timing.h" |
|
||||||
#include "selfdrive/common/util.h" |
|
||||||
|
|
||||||
// id of the video capturing device
|
|
||||||
const int ROAD_CAMERA_ID = util::getenv("ROADCAM_ID", 1); |
|
||||||
const int DRIVER_CAMERA_ID = util::getenv("DRIVERCAM_ID", 2); |
|
||||||
|
|
||||||
#define FRAME_WIDTH 1164 |
|
||||||
#define FRAME_HEIGHT 874 |
|
||||||
#define FRAME_WIDTH_FRONT 1152 |
|
||||||
#define FRAME_HEIGHT_FRONT 864 |
|
||||||
|
|
||||||
extern ExitHandler do_exit; |
|
||||||
|
|
||||||
namespace { |
|
||||||
|
|
||||||
CameraInfo cameras_supported[CAMERA_ID_MAX] = { |
|
||||||
// road facing
|
|
||||||
[CAMERA_ID_LGC920] = { |
|
||||||
.frame_width = FRAME_WIDTH, |
|
||||||
.frame_height = FRAME_HEIGHT, |
|
||||||
.frame_stride = FRAME_WIDTH*3, |
|
||||||
.bayer = false, |
|
||||||
.bayer_flip = false, |
|
||||||
}, |
|
||||||
// driver facing
|
|
||||||
[CAMERA_ID_LGC615] = { |
|
||||||
.frame_width = FRAME_WIDTH_FRONT, |
|
||||||
.frame_height = FRAME_HEIGHT_FRONT, |
|
||||||
.frame_stride = FRAME_WIDTH_FRONT*3, |
|
||||||
.bayer = false, |
|
||||||
.bayer_flip = false, |
|
||||||
}, |
|
||||||
}; |
|
||||||
|
|
||||||
void camera_open(CameraState *s, bool rear) { |
|
||||||
// empty
|
|
||||||
} |
|
||||||
|
|
||||||
void camera_close(CameraState *s) { |
|
||||||
// empty
|
|
||||||
} |
|
||||||
|
|
||||||
void camera_init(VisionIpcServer * v, CameraState *s, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) { |
|
||||||
assert(camera_id < std::size(cameras_supported)); |
|
||||||
s->ci = cameras_supported[camera_id]; |
|
||||||
assert(s->ci.frame_width != 0); |
|
||||||
|
|
||||||
s->camera_num = camera_id; |
|
||||||
s->fps = fps; |
|
||||||
s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type); |
|
||||||
} |
|
||||||
|
|
||||||
void run_camera(CameraState *s, cv::VideoCapture &video_cap, float *ts) { |
|
||||||
assert(video_cap.isOpened()); |
|
||||||
|
|
||||||
cv::Size size(s->ci.frame_width, s->ci.frame_height); |
|
||||||
const cv::Mat transform = cv::Mat(3, 3, CV_32F, ts); |
|
||||||
uint32_t frame_id = 0; |
|
||||||
size_t buf_idx = 0; |
|
||||||
|
|
||||||
while (!do_exit) { |
|
||||||
cv::Mat frame_mat, transformed_mat; |
|
||||||
video_cap >> frame_mat; |
|
||||||
if (frame_mat.empty()) continue; |
|
||||||
|
|
||||||
cv::warpPerspective(frame_mat, transformed_mat, transform, size, cv::INTER_LINEAR, cv::BORDER_CONSTANT, 0); |
|
||||||
|
|
||||||
s->buf.camera_bufs_metadata[buf_idx] = {.frame_id = frame_id}; |
|
||||||
|
|
||||||
auto &buf = s->buf.camera_bufs[buf_idx]; |
|
||||||
int transformed_size = transformed_mat.total() * transformed_mat.elemSize(); |
|
||||||
CL_CHECK(clEnqueueWriteBuffer(buf.copy_q, buf.buf_cl, CL_TRUE, 0, transformed_size, transformed_mat.data, 0, NULL, NULL)); |
|
||||||
|
|
||||||
s->buf.queue(buf_idx); |
|
||||||
|
|
||||||
++frame_id; |
|
||||||
buf_idx = (buf_idx + 1) % FRAME_BUF_COUNT; |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
static void road_camera_thread(CameraState *s) { |
|
||||||
util::set_thread_name("webcam_road_camera_thread"); |
|
||||||
|
|
||||||
cv::VideoCapture cap_road(ROAD_CAMERA_ID, cv::CAP_V4L2); // road
|
|
||||||
cap_road.set(cv::CAP_PROP_FRAME_WIDTH, 853); |
|
||||||
cap_road.set(cv::CAP_PROP_FRAME_HEIGHT, 480); |
|
||||||
cap_road.set(cv::CAP_PROP_FPS, s->fps); |
|
||||||
cap_road.set(cv::CAP_PROP_AUTOFOCUS, 0); // off
|
|
||||||
cap_road.set(cv::CAP_PROP_FOCUS, 0); // 0 - 255?
|
|
||||||
// cv::Rect roi_rear(160, 0, 960, 720);
|
|
||||||
|
|
||||||
// transforms calculation see tools/webcam/warp_vis.py
|
|
||||||
float ts[9] = {1.50330396, 0.0, -59.40969163, |
|
||||||
0.0, 1.50330396, 76.20704846, |
|
||||||
0.0, 0.0, 1.0}; |
|
||||||
// if camera upside down:
|
|
||||||
// float ts[9] = {-1.50330396, 0.0, 1223.4,
|
|
||||||
// 0.0, -1.50330396, 797.8,
|
|
||||||
// 0.0, 0.0, 1.0};
|
|
||||||
|
|
||||||
run_camera(s, cap_road, ts); |
|
||||||
} |
|
||||||
|
|
||||||
void driver_camera_thread(CameraState *s) { |
|
||||||
cv::VideoCapture cap_driver(DRIVER_CAMERA_ID, cv::CAP_V4L2); // driver
|
|
||||||
cap_driver.set(cv::CAP_PROP_FRAME_WIDTH, 853); |
|
||||||
cap_driver.set(cv::CAP_PROP_FRAME_HEIGHT, 480); |
|
||||||
cap_driver.set(cv::CAP_PROP_FPS, s->fps); |
|
||||||
// cv::Rect roi_front(320, 0, 960, 720);
|
|
||||||
|
|
||||||
// transforms calculation see tools/webcam/warp_vis.py
|
|
||||||
float ts[9] = {1.42070485, 0.0, -30.16740088, |
|
||||||
0.0, 1.42070485, 91.030837, |
|
||||||
0.0, 0.0, 1.0}; |
|
||||||
// if camera upside down:
|
|
||||||
// float ts[9] = {-1.42070485, 0.0, 1182.2,
|
|
||||||
// 0.0, -1.42070485, 773.0,
|
|
||||||
// 0.0, 0.0, 1.0};
|
|
||||||
run_camera(s, cap_driver, ts); |
|
||||||
} |
|
||||||
|
|
||||||
} // namespace
|
|
||||||
|
|
||||||
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) { |
|
||||||
camera_init(v, &s->road_cam, CAMERA_ID_LGC920, 20, device_id, ctx, |
|
||||||
VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD); |
|
||||||
camera_init(v, &s->driver_cam, CAMERA_ID_LGC615, 10, device_id, ctx, |
|
||||||
VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER); |
|
||||||
s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"}); |
|
||||||
} |
|
||||||
|
|
||||||
void camera_autoexposure(CameraState *s, float grey_frac) {} |
|
||||||
|
|
||||||
void cameras_open(MultiCameraState *s) { |
|
||||||
// LOG("*** open driver camera ***");
|
|
||||||
camera_open(&s->driver_cam, false); |
|
||||||
// LOG("*** open road camera ***");
|
|
||||||
camera_open(&s->road_cam, true); |
|
||||||
} |
|
||||||
|
|
||||||
void cameras_close(MultiCameraState *s) { |
|
||||||
camera_close(&s->road_cam); |
|
||||||
camera_close(&s->driver_cam); |
|
||||||
delete s->pm; |
|
||||||
} |
|
||||||
|
|
||||||
void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) { |
|
||||||
MessageBuilder msg; |
|
||||||
auto framed = msg.initEvent().initDriverCameraState(); |
|
||||||
framed.setFrameType(cereal::FrameData::FrameType::FRONT); |
|
||||||
fill_frame_data(framed, c->buf.cur_frame_data); |
|
||||||
s->pm->send("driverCameraState", msg); |
|
||||||
} |
|
||||||
|
|
||||||
void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { |
|
||||||
const CameraBuf *b = &c->buf; |
|
||||||
MessageBuilder msg; |
|
||||||
auto framed = msg.initEvent().initRoadCameraState(); |
|
||||||
fill_frame_data(framed, b->cur_frame_data); |
|
||||||
framed.setImage(kj::arrayPtr((const uint8_t *)b->cur_yuv_buf->addr, b->cur_yuv_buf->len)); |
|
||||||
framed.setTransform(b->yuv_transform.v); |
|
||||||
s->pm->send("roadCameraState", msg); |
|
||||||
} |
|
||||||
|
|
||||||
void cameras_run(MultiCameraState *s) { |
|
||||||
std::vector<std::thread> threads; |
|
||||||
threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera)); |
|
||||||
threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_camera)); |
|
||||||
|
|
||||||
std::thread t_rear = std::thread(road_camera_thread, &s->road_cam); |
|
||||||
util::set_thread_name("webcam_thread"); |
|
||||||
driver_camera_thread(&s->driver_cam); |
|
||||||
|
|
||||||
t_rear.join(); |
|
||||||
|
|
||||||
for (auto &t : threads) t.join(); |
|
||||||
|
|
||||||
cameras_close(s); |
|
||||||
} |
|
||||||
@ -1,28 +0,0 @@ |
|||||||
#pragma once |
|
||||||
|
|
||||||
#ifdef __APPLE__ |
|
||||||
#include <OpenCL/cl.h> |
|
||||||
#else |
|
||||||
#include <CL/cl.h> |
|
||||||
#endif |
|
||||||
|
|
||||||
#include "selfdrive/camerad/cameras/camera_common.h" |
|
||||||
|
|
||||||
#define FRAME_BUF_COUNT 16 |
|
||||||
|
|
||||||
typedef struct CameraState { |
|
||||||
CameraInfo ci; |
|
||||||
int camera_num; |
|
||||||
int fps; |
|
||||||
float digital_gain; |
|
||||||
CameraBuf buf; |
|
||||||
} CameraState; |
|
||||||
|
|
||||||
|
|
||||||
typedef struct MultiCameraState { |
|
||||||
CameraState road_cam; |
|
||||||
CameraState driver_cam; |
|
||||||
|
|
||||||
SubMaster *sm; |
|
||||||
PubMaster *pm; |
|
||||||
} MultiCameraState; |
|
||||||
@ -1,189 +1,157 @@ |
|||||||
#ifdef HALF_AS_FLOAT |
#define UV_WIDTH RGB_WIDTH / 2 |
||||||
#define half float |
#define UV_HEIGHT RGB_HEIGHT / 2 |
||||||
#define half3 float3 |
#define U_OFFSET RGB_WIDTH * RGB_HEIGHT |
||||||
#else |
#define V_OFFSET RGB_WIDTH * RGB_HEIGHT + UV_WIDTH * UV_HEIGHT |
||||||
#pragma OPENCL EXTENSION cl_khr_fp16 : enable |
|
||||||
#endif |
#define RGB_TO_Y(r, g, b) ((((mul24(b, 13) + mul24(g, 65) + mul24(r, 33)) + 64) >> 7) + 16) |
||||||
|
#define RGB_TO_U(r, g, b) ((mul24(b, 56) - mul24(g, 37) - mul24(r, 19) + 0x8080) >> 8) |
||||||
// post wb CCM |
#define RGB_TO_V(r, g, b) ((mul24(r, 56) - mul24(g, 47) - mul24(b, 9) + 0x8080) >> 8) |
||||||
const __constant half3 color_correction_0 = (half3)(1.82717181, -0.31231438, 0.07307673); |
#define AVERAGE(x, y, z, w) ((convert_ushort(x) + convert_ushort(y) + convert_ushort(z) + convert_ushort(w) + 1) >> 1) |
||||||
const __constant half3 color_correction_1 = (half3)(-0.5743977, 1.36858544, -0.53183455); |
|
||||||
const __constant half3 color_correction_2 = (half3)(-0.25277411, -0.05627105, 1.45875782); |
float3 color_correct(float3 rgb) { |
||||||
|
// color correction |
||||||
// tone mapping params |
float3 x = rgb.x * (float3)(1.82717181, -0.31231438, 0.07307673); |
||||||
const half gamma_k = 0.75; |
x += rgb.y * (float3)(-0.5743977, 1.36858544, -0.53183455); |
||||||
const half gamma_b = 0.125; |
x += rgb.z * (float3)(-0.25277411, -0.05627105, 1.45875782); |
||||||
const half mp = 0.01; // ideally midpoint should be adaptive |
|
||||||
const half rk = 9 - 100*mp; |
// tone mapping params |
||||||
|
const float gamma_k = 0.75; |
||||||
inline half3 gamma_apply(half3 x) { |
const float gamma_b = 0.125; |
||||||
|
const float mp = 0.01; // ideally midpoint should be adaptive |
||||||
|
const float rk = 9 - 100*mp; |
||||||
|
|
||||||
// poly approximation for s curve |
// poly approximation for s curve |
||||||
return (x > mp) ? |
return (x > mp) ? |
||||||
((rk * (x-mp) * (1-(gamma_k*mp+gamma_b)) * (1+1/(rk*(1-mp))) / (1+rk*(x-mp))) + gamma_k*mp + gamma_b) : |
((rk * (x-mp) * (1-(gamma_k*mp+gamma_b)) * (1+1/(rk*(1-mp))) / (1+rk*(x-mp))) + gamma_k*mp + gamma_b) : |
||||||
((rk * (x-mp) * (gamma_k*mp+gamma_b) * (1+1/(rk*mp)) / (1-rk*(x-mp))) + gamma_k*mp + gamma_b); |
((rk * (x-mp) * (gamma_k*mp+gamma_b) * (1+1/(rk*mp)) / (1-rk*(x-mp))) + gamma_k*mp + gamma_b); |
||||||
} |
} |
||||||
|
|
||||||
inline half3 color_correct(half3 rgb) { |
float get_vignetting_s(float r) { |
||||||
half3 ret = (half)rgb.x * color_correction_0; |
|
||||||
ret += (half)rgb.y * color_correction_1; |
|
||||||
ret += (half)rgb.z * color_correction_2; |
|
||||||
return gamma_apply(ret); |
|
||||||
} |
|
||||||
|
|
||||||
inline half get_vignetting_s(float r) { |
|
||||||
if (r < 62500) { |
if (r < 62500) { |
||||||
return (half)(1.0f + 0.0000008f*r); |
return (1.0f + 0.0000008f*r); |
||||||
} else if (r < 490000) { |
} else if (r < 490000) { |
||||||
return (half)(0.9625f + 0.0000014f*r); |
return (0.9625f + 0.0000014f*r); |
||||||
} else if (r < 1102500) { |
} else if (r < 1102500) { |
||||||
return (half)(1.26434f + 0.0000000000016f*r*r); |
return (1.26434f + 0.0000000000016f*r*r); |
||||||
} else { |
} else { |
||||||
return (half)(0.53503625f + 0.0000000000022f*r*r); |
return (0.53503625f + 0.0000000000022f*r*r); |
||||||
} |
} |
||||||
} |
} |
||||||
|
|
||||||
inline half val_from_10(const uchar * source, int gx, int gy, half black_level) { |
float4 val4_from_12(uchar8 pvs, float gain) { |
||||||
// parse 12bit |
uint4 parsed = (uint4)(((uint)pvs.s0<<4) + (pvs.s1>>4), // is from the previous 10 bit |
||||||
int start = gy * FRAME_STRIDE + (3 * (gx / 2)) + (FRAME_STRIDE * FRAME_OFFSET); |
((uint)pvs.s2<<4) + (pvs.s4&0xF), |
||||||
int offset = gx % 2; |
((uint)pvs.s3<<4) + (pvs.s4>>4), |
||||||
uint major = (uint)source[start + offset] << 4; |
((uint)pvs.s5<<4) + (pvs.s7&0xF)); |
||||||
uint minor = (source[start + 2] >> (4 * offset)) & 0xf; |
// normalize and scale |
||||||
half pv = (half)((major + minor)/4); |
float4 pv = (convert_float4(parsed) - 168.0) / (4096.0 - 168.0); |
||||||
|
return clamp(pv*gain, 0.0, 1.0); |
||||||
// normalize |
|
||||||
pv = max((half)0.0, pv - black_level); |
|
||||||
pv /= (1024.0 - black_level); |
|
||||||
|
|
||||||
// correct vignetting |
|
||||||
if (CAM_NUM == 1) { // fcamera |
|
||||||
gx = (gx - RGB_WIDTH/2); |
|
||||||
gy = (gy - RGB_HEIGHT/2); |
|
||||||
pv *= get_vignetting_s(gx*gx + gy*gy); |
|
||||||
} |
|
||||||
|
|
||||||
pv = clamp(pv, (half)0.0, (half)1.0); |
|
||||||
return pv; |
|
||||||
} |
} |
||||||
|
|
||||||
inline half get_k(half a, half b, half c, half d) { |
float get_k(float a, float b, float c, float d) { |
||||||
return 2.0 - (fabs(a - b) + fabs(c - d)); |
return 2.0 - (fabs(a - b) + fabs(c - d)); |
||||||
} |
} |
||||||
|
|
||||||
__kernel void debayer10(const __global uchar * in, |
__kernel void debayer10(const __global uchar * in, __global uchar * out) |
||||||
__global uchar * out, |
|
||||||
__local half * cached, |
|
||||||
float black_level |
|
||||||
) |
|
||||||
{ |
{ |
||||||
const int x_global = get_global_id(0); |
const int gid_x = get_global_id(0); |
||||||
const int y_global = get_global_id(1); |
const int gid_y = get_global_id(1); |
||||||
|
|
||||||
const int x_local = get_local_id(0); |
|
||||||
const int y_local = get_local_id(1); |
|
||||||
|
|
||||||
const int localRowLen = 2 + get_local_size(0); // 2 padding |
|
||||||
const int localColLen = 2 + get_local_size(1); |
|
||||||
|
|
||||||
const int localOffset = (y_local + 1) * localRowLen + x_local + 1; |
const int y_top_mod = (gid_y == 0) ? 2: 0; |
||||||
|
const int y_bot_mod = (gid_y == (RGB_HEIGHT/2 - 1)) ? 1: 3; |
||||||
|
|
||||||
int out_idx = 3 * x_global + 3 * y_global * RGB_WIDTH; |
float3 rgb; |
||||||
|
uchar3 rgb_out[4]; |
||||||
|
|
||||||
// cache padding |
int start = (2 * gid_y - 1) * FRAME_STRIDE + (3 * gid_x - 2) + (FRAME_STRIDE * FRAME_OFFSET); |
||||||
int localColOffset = -1; |
|
||||||
int globalColOffset; |
|
||||||
|
|
||||||
const int x_global_mod = (x_global == 0 || x_global == RGB_WIDTH - 1) ? -1: 1; |
// read in 8x4 chars |
||||||
const int y_global_mod = (y_global == 0 || y_global == RGB_HEIGHT - 1) ? -1: 1; |
uchar8 dat[4]; |
||||||
|
dat[0] = vload8(0, in + start + FRAME_STRIDE*y_top_mod); |
||||||
half pv = val_from_10(in, x_global, y_global, black_level); |
dat[1] = vload8(0, in + start + FRAME_STRIDE*1); |
||||||
cached[localOffset] = pv; |
dat[2] = vload8(0, in + start + FRAME_STRIDE*2); |
||||||
|
dat[3] = vload8(0, in + start + FRAME_STRIDE*y_bot_mod); |
||||||
// cache padding |
|
||||||
if (x_local < 1) { |
|
||||||
localColOffset = x_local; |
|
||||||
globalColOffset = -1; |
|
||||||
cached[(y_local + 1) * localRowLen + x_local] = val_from_10(in, x_global-x_global_mod, y_global, black_level); |
|
||||||
} else if (x_local >= get_local_size(0) - 1) { |
|
||||||
localColOffset = x_local + 2; |
|
||||||
globalColOffset = 1; |
|
||||||
cached[localOffset + 1] = val_from_10(in, x_global+x_global_mod, y_global, black_level); |
|
||||||
} |
|
||||||
|
|
||||||
if (y_local < 1) { |
// correct vignetting |
||||||
cached[y_local * localRowLen + x_local + 1] = val_from_10(in, x_global, y_global-y_global_mod, black_level); |
#if VIGNETTING |
||||||
if (localColOffset != -1) { |
int gx = (gid_x*2 - RGB_WIDTH/2); |
||||||
cached[y_local * localRowLen + localColOffset] = val_from_10(in, x_global+(x_global_mod*globalColOffset), y_global-y_global_mod, black_level); |
int gy = (gid_y*2 - RGB_HEIGHT/2); |
||||||
} |
const float gain = get_vignetting_s(gx*gx + gy*gy); |
||||||
} else if (y_local >= get_local_size(1) - 1) { |
#else |
||||||
cached[(y_local + 2) * localRowLen + x_local + 1] = val_from_10(in, x_global, y_global+y_global_mod, black_level); |
const float gain = 1.0; |
||||||
if (localColOffset != -1) { |
#endif |
||||||
cached[(y_local + 2) * localRowLen + localColOffset] = val_from_10(in, x_global+(x_global_mod*globalColOffset), y_global+y_global_mod, black_level); |
|
||||||
} |
// process them to floats |
||||||
|
float4 va = val4_from_12(dat[0], gain); |
||||||
|
float4 vb = val4_from_12(dat[1], gain); |
||||||
|
float4 vc = val4_from_12(dat[2], gain); |
||||||
|
float4 vd = val4_from_12(dat[3], gain); |
||||||
|
|
||||||
|
if (gid_x == 0) { |
||||||
|
va.s0 = va.s2; |
||||||
|
vb.s0 = vb.s2; |
||||||
|
vc.s0 = vc.s2; |
||||||
|
vd.s0 = vd.s2; |
||||||
|
} else if (gid_x == RGB_WIDTH/2 - 1) { |
||||||
|
va.s3 = va.s1; |
||||||
|
vb.s3 = vb.s1; |
||||||
|
vc.s3 = vc.s1; |
||||||
|
vd.s3 = vd.s1; |
||||||
} |
} |
||||||
|
|
||||||
// sync |
|
||||||
barrier(CLK_LOCAL_MEM_FENCE); |
|
||||||
|
|
||||||
half d1 = cached[localOffset - localRowLen - 1]; |
|
||||||
half d2 = cached[localOffset - localRowLen + 1]; |
|
||||||
half d3 = cached[localOffset + localRowLen - 1]; |
|
||||||
half d4 = cached[localOffset + localRowLen + 1]; |
|
||||||
half n1 = cached[localOffset - localRowLen]; |
|
||||||
half n2 = cached[localOffset + 1]; |
|
||||||
half n3 = cached[localOffset + localRowLen]; |
|
||||||
half n4 = cached[localOffset - 1]; |
|
||||||
|
|
||||||
half3 rgb; |
|
||||||
|
|
||||||
// a simplified version of https://opensignalprocessingjournal.com/contents/volumes/V6/TOSIGPJ-6-1/TOSIGPJ-6-1.pdf |
// a simplified version of https://opensignalprocessingjournal.com/contents/volumes/V6/TOSIGPJ-6-1/TOSIGPJ-6-1.pdf |
||||||
if (x_global % 2 == 0) { |
const float k01 = get_k(va.s0, vb.s1, va.s2, vb.s1); |
||||||
if (y_global % 2 == 0) { |
const float k02 = get_k(va.s2, vb.s1, vc.s2, vb.s1); |
||||||
rgb.y = pv; // G1(R) |
const float k03 = get_k(vc.s0, vb.s1, vc.s2, vb.s1); |
||||||
half k1 = get_k(d1, pv, d2, pv); |
const float k04 = get_k(va.s0, vb.s1, vc.s0, vb.s1); |
||||||
half k2 = get_k(d2, pv, d4, pv); |
rgb.x = (k02*vb.s2+k04*vb.s0)/(k02+k04); // R_G1 |
||||||
half k3 = get_k(d3, pv, d4, pv); |
rgb.y = vb.s1; // G1(R) |
||||||
half k4 = get_k(d1, pv, d3, pv); |
rgb.z = (k01*va.s1+k03*vc.s1)/(k01+k03); // B_G1 |
||||||
// R_G1 |
rgb_out[0] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); |
||||||
rgb.x = (k2*n2+k4*n4)/(k2+k4); |
|
||||||
// B_G1 |
const float k11 = get_k(va.s1, vc.s1, va.s3, vc.s3); |
||||||
rgb.z = (k1*n1+k3*n3)/(k1+k3); |
const float k12 = get_k(va.s2, vb.s1, vb.s3, vc.s2); |
||||||
} else { |
const float k13 = get_k(va.s1, va.s3, vc.s1, vc.s3); |
||||||
rgb.z = pv; // B |
const float k14 = get_k(va.s2, vb.s3, vc.s2, vb.s1); |
||||||
half k1 = get_k(d1, d3, d2, d4); |
rgb.x = vb.s2; // R |
||||||
half k2 = get_k(n1, n4, n2, n3); |
rgb.y = (k11*(va.s2+vc.s2)*0.5+k13*(vb.s3+vb.s1)*0.5)/(k11+k13); // G_R |
||||||
half k3 = get_k(d1, d2, d3, d4); |
rgb.z = (k12*(va.s3+vc.s1)*0.5+k14*(va.s1+vc.s3)*0.5)/(k12+k14); // B_R |
||||||
half k4 = get_k(n1, n2, n3, n4); |
rgb_out[1] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); |
||||||
// G_B |
|
||||||
rgb.y = (k1*(n1+n3)*0.5+k3*(n2+n4)*0.5)/(k1+k3); |
const float k21 = get_k(vb.s0, vd.s0, vb.s2, vd.s2); |
||||||
// R_B |
const float k22 = get_k(vb.s1, vc.s0, vc.s2, vd.s1); |
||||||
rgb.x = (k2*(d2+d3)*0.5+k4*(d1+d4)*0.5)/(k2+k4); |
const float k23 = get_k(vb.s0, vb.s2, vd.s0, vd.s2); |
||||||
} |
const float k24 = get_k(vb.s1, vc.s2, vd.s1, vc.s0); |
||||||
} else { |
rgb.x = (k22*(vb.s2+vd.s0)*0.5+k24*(vb.s0+vd.s2)*0.5)/(k22+k24); // R_B |
||||||
if (y_global % 2 == 0) { |
rgb.y = (k21*(vb.s1+vd.s1)*0.5+k23*(vc.s2+vc.s0)*0.5)/(k21+k23); // G_B |
||||||
rgb.x = pv; // R |
rgb.z = vc.s1; // B |
||||||
half k1 = get_k(d1, d3, d2, d4); |
rgb_out[2] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); |
||||||
half k2 = get_k(n1, n4, n2, n3); |
|
||||||
half k3 = get_k(d1, d2, d3, d4); |
const float k31 = get_k(vb.s1, vc.s2, vb.s3, vc.s2); |
||||||
half k4 = get_k(n1, n2, n3, n4); |
const float k32 = get_k(vb.s3, vc.s2, vd.s3, vc.s2); |
||||||
// G_R |
const float k33 = get_k(vd.s1, vc.s2, vd.s3, vc.s2); |
||||||
rgb.y = (k1*(n1+n3)*0.5+k3*(n2+n4)*0.5)/(k1+k3); |
const float k34 = get_k(vb.s1, vc.s2, vd.s1, vc.s2); |
||||||
// B_R |
rgb.x = (k31*vb.s2+k33*vd.s2)/(k31+k33); // R_G2 |
||||||
rgb.z = (k2*(d2+d3)*0.5+k4*(d1+d4)*0.5)/(k2+k4); |
rgb.y = vc.s2; // G2(B) |
||||||
} else { |
rgb.z = (k32*vc.s3+k34*vc.s1)/(k32+k34); // B_G2 |
||||||
rgb.y = pv; // G2(B) |
rgb_out[3] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); |
||||||
half k1 = get_k(d1, pv, d2, pv); |
|
||||||
half k2 = get_k(d2, pv, d4, pv); |
// write ys |
||||||
half k3 = get_k(d3, pv, d4, pv); |
uchar2 yy = (uchar2)( |
||||||
half k4 = get_k(d1, pv, d3, pv); |
RGB_TO_Y(rgb_out[0].s0, rgb_out[0].s1, rgb_out[0].s2), |
||||||
// R_G2 |
RGB_TO_Y(rgb_out[1].s0, rgb_out[1].s1, rgb_out[1].s2) |
||||||
rgb.x = (k1*n1+k3*n3)/(k1+k3); |
); |
||||||
// B_G2 |
vstore2(yy, 0, out + mad24(gid_y * 2, RGB_WIDTH, gid_x * 2)); |
||||||
rgb.z = (k2*n2+k4*n4)/(k2+k4); |
yy = (uchar2)( |
||||||
} |
RGB_TO_Y(rgb_out[2].s0, rgb_out[2].s1, rgb_out[2].s2), |
||||||
} |
RGB_TO_Y(rgb_out[3].s0, rgb_out[3].s1, rgb_out[3].s2) |
||||||
|
); |
||||||
uchar3 rgbc = convert_uchar3_sat(color_correct(clamp(rgb, (half)0.0, (half)1.0)) * 255.0); |
vstore2(yy, 0, out + mad24(gid_y * 2 + 1, RGB_WIDTH, gid_x * 2)); |
||||||
out[out_idx + 0] = rgbc.z; |
|
||||||
out[out_idx + 1] = rgbc.y; |
// write uvs |
||||||
out[out_idx + 2] = rgbc.x; |
const short ar = AVERAGE(rgb_out[0].s0, rgb_out[1].s0, rgb_out[2].s0, rgb_out[3].s0); |
||||||
|
const short ag = AVERAGE(rgb_out[0].s1, rgb_out[1].s1, rgb_out[2].s1, rgb_out[3].s1); |
||||||
|
const short ab = AVERAGE(rgb_out[0].s2, rgb_out[1].s2, rgb_out[2].s2, rgb_out[3].s2); |
||||||
|
out[U_OFFSET + mad24(gid_y, UV_WIDTH, gid_x)] = RGB_TO_U(ar, ag, ab); |
||||||
|
out[V_OFFSET + mad24(gid_y, UV_WIDTH, gid_x)] = RGB_TO_V(ar, ag, ab); |
||||||
} |
} |
||||||
|
|||||||
File diff suppressed because one or more lines are too long
@ -0,0 +1,26 @@ |
|||||||
|
// TODO: cleanup AE tests
|
||||||
|
// needed by camera_common.cc
|
||||||
|
void camera_autoexposure(CameraState *s, float grey_frac) {} |
||||||
|
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {} |
||||||
|
void cameras_open(MultiCameraState *s) {} |
||||||
|
void cameras_run(MultiCameraState *s) {} |
||||||
|
|
||||||
|
typedef struct CameraState { |
||||||
|
int camera_num; |
||||||
|
CameraInfo ci; |
||||||
|
|
||||||
|
int fps; |
||||||
|
float digital_gain = 0; |
||||||
|
|
||||||
|
CameraBuf buf; |
||||||
|
} CameraState; |
||||||
|
|
||||||
|
typedef struct MultiCameraState { |
||||||
|
CameraState road_cam; |
||||||
|
CameraState driver_cam; |
||||||
|
|
||||||
|
SubMaster *sm = nullptr; |
||||||
|
PubMaster *pm = nullptr; |
||||||
|
} MultiCameraState; |
||||||
|
|
||||||
|
|
||||||
@ -1,35 +0,0 @@ |
|||||||
Import('env', 'arch', 'SHARED') |
|
||||||
|
|
||||||
if SHARED: |
|
||||||
fxn = env.SharedLibrary |
|
||||||
else: |
|
||||||
fxn = env.Library |
|
||||||
|
|
||||||
common_libs = [ |
|
||||||
'params.cc', |
|
||||||
'statlog.cc', |
|
||||||
'swaglog.cc', |
|
||||||
'util.cc', |
|
||||||
'gpio.cc', |
|
||||||
'i2c.cc', |
|
||||||
'watchdog.cc', |
|
||||||
] |
|
||||||
|
|
||||||
_common = fxn('common', common_libs, LIBS="json11") |
|
||||||
|
|
||||||
files = [ |
|
||||||
'clutil.cc', |
|
||||||
'visionimg.cc', |
|
||||||
] |
|
||||||
|
|
||||||
if arch == "larch64": |
|
||||||
_gpu_libs = ["GLESv2"] |
|
||||||
else: |
|
||||||
_gpu_libs = ["GL"] |
|
||||||
|
|
||||||
_gpucommon = fxn('gpucommon', files, LIBS=_gpu_libs) |
|
||||||
Export('_common', '_gpucommon', '_gpu_libs') |
|
||||||
|
|
||||||
if GetOption('test'): |
|
||||||
env.Program('tests/test_util', ['tests/test_util.cc'], LIBS=[_common]) |
|
||||||
env.Program('tests/test_swaglog', ['tests/test_swaglog.cc'], LIBS=[_common, 'json11', 'zmq', 'pthread']) |
|
||||||
@ -0,0 +1,84 @@ |
|||||||
|
import math |
||||||
|
import numpy as np |
||||||
|
|
||||||
|
from common.numpy_fast import clip |
||||||
|
from common.realtime import DT_CTRL |
||||||
|
from cereal import log |
||||||
|
from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED |
||||||
|
|
||||||
|
|
||||||
|
class LatControlLQR(LatControl): |
||||||
|
def __init__(self, CP, CI): |
||||||
|
super().__init__(CP, CI) |
||||||
|
self.scale = CP.lateralTuning.lqr.scale |
||||||
|
self.ki = CP.lateralTuning.lqr.ki |
||||||
|
|
||||||
|
self.A = np.array(CP.lateralTuning.lqr.a).reshape((2, 2)) |
||||||
|
self.B = np.array(CP.lateralTuning.lqr.b).reshape((2, 1)) |
||||||
|
self.C = np.array(CP.lateralTuning.lqr.c).reshape((1, 2)) |
||||||
|
self.K = np.array(CP.lateralTuning.lqr.k).reshape((1, 2)) |
||||||
|
self.L = np.array(CP.lateralTuning.lqr.l).reshape((2, 1)) |
||||||
|
self.dc_gain = CP.lateralTuning.lqr.dcGain |
||||||
|
|
||||||
|
self.x_hat = np.array([[0], [0]]) |
||||||
|
self.i_unwind_rate = 0.3 * DT_CTRL |
||||||
|
self.i_rate = 1.0 * DT_CTRL |
||||||
|
|
||||||
|
self.reset() |
||||||
|
|
||||||
|
def reset(self): |
||||||
|
super().reset() |
||||||
|
self.i_lqr = 0.0 |
||||||
|
|
||||||
|
def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): |
||||||
|
lqr_log = log.ControlsState.LateralLQRState.new_message() |
||||||
|
|
||||||
|
torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed |
||||||
|
|
||||||
|
# Subtract offset. Zero angle should correspond to zero torque |
||||||
|
steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg |
||||||
|
|
||||||
|
desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) |
||||||
|
|
||||||
|
instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg |
||||||
|
desired_angle += instant_offset # Only add offset that originates from vehicle model errors |
||||||
|
lqr_log.steeringAngleDesiredDeg = desired_angle |
||||||
|
|
||||||
|
# Update Kalman filter |
||||||
|
angle_steers_k = float(self.C.dot(self.x_hat)) |
||||||
|
e = steering_angle_no_offset - angle_steers_k |
||||||
|
self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e) |
||||||
|
|
||||||
|
if CS.vEgo < MIN_STEER_SPEED or not active: |
||||||
|
lqr_log.active = False |
||||||
|
lqr_output = 0. |
||||||
|
output_steer = 0. |
||||||
|
self.reset() |
||||||
|
else: |
||||||
|
lqr_log.active = True |
||||||
|
|
||||||
|
# LQR |
||||||
|
u_lqr = float(desired_angle / self.dc_gain - self.K.dot(self.x_hat)) |
||||||
|
lqr_output = torque_scale * u_lqr / self.scale |
||||||
|
|
||||||
|
# Integrator |
||||||
|
if CS.steeringPressed: |
||||||
|
self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr)) |
||||||
|
else: |
||||||
|
error = desired_angle - angle_steers_k |
||||||
|
i = self.i_lqr + self.ki * self.i_rate * error |
||||||
|
control = lqr_output + i |
||||||
|
|
||||||
|
if (error >= 0 and (control <= self.steer_max or i < 0.0)) or \ |
||||||
|
(error <= 0 and (control >= -self.steer_max or i > 0.0)): |
||||||
|
self.i_lqr = i |
||||||
|
|
||||||
|
output_steer = lqr_output + self.i_lqr |
||||||
|
output_steer = clip(output_steer, -self.steer_max, self.steer_max) |
||||||
|
|
||||||
|
lqr_log.steeringAngleDeg = angle_steers_k |
||||||
|
lqr_log.i = self.i_lqr |
||||||
|
lqr_log.output = output_steer |
||||||
|
lqr_log.lqrOutput = lqr_output |
||||||
|
lqr_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS) |
||||||
|
return output_steer, desired_angle, lqr_log |
||||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue