diff --git a/main.cpp b/main.cpp index 509f4ee..2242478 100644 --- a/main.cpp +++ b/main.cpp @@ -1,108 +1,330 @@ -#include -#include -#include -#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include + +#include +#include #include +#include +#include +#include +#include +#include +#include using namespace libcamera; -using namespace std::chrono_literals; static std::atomic capturing{true}; - static std::shared_ptr camera; +static std::mutex frameMutex; +static std::condition_variable frameCv; +static std::queue done; +static int clientFd = -1; + +struct MappedBuffer { void *mem; size_t len; }; +static std::unordered_map mapped; + +enum class Mode { Motion, Raw }; + +static Mode parseMode(int argc, char **argv) +{ + if (argc < 2) + return Mode::Motion; + if (!strcmp(argv[1], "--raw") || !strcmp(argv[1], "raw")) + return Mode::Raw; + if (!strcmp(argv[1], "--motion") || !strcmp(argv[1], "motion")) + return Mode::Motion; + std::cerr << "Usage: " << argv[0] << " [--motion|--raw]\n"; + exit(1); +} + +static int waitForClient(int port, const char *label) +{ + int server = socket(AF_INET, SOCK_STREAM, 0); + int yes = 1; + setsockopt(server, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(yes)); + + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = INADDR_ANY; + addr.sin_port = htons(port); + bind(server, reinterpret_cast(&addr), sizeof(addr)); + listen(server, 1); + std::cerr << "tcp://0.0.0.0:" << port << " waiting...\n"; + + int client = accept(server, nullptr, nullptr); + close(server); + setsockopt(client, IPPROTO_TCP, TCP_NODELAY, &yes, sizeof(yes)); + std::cerr << "streaming " << label << "\n"; + return client; +} + +static bool sendAll(const unsigned char *data, size_t size) +{ + while (size && capturing) { + ssize_t n = send(clientFd, data, size, MSG_NOSIGNAL); + if (n <= 0) + return false; + data += n; + size -= n; + } + return true; +} static void requestComplete(Request *request) { if (request->status() == Request::RequestCancelled) return; - - for (auto const &bufferPair : request->buffers()) { - FrameBuffer *buffer = bufferPair.second; - const FrameMetadata &metadata = buffer->metadata(); - if (metadata.status != FrameMetadata::FrameSuccess) - continue; - - int fd = buffer->planes()[0].fd.get(); // Import this dma-buf fd into EGL/OpenGL. - std::cout << "frame " << std::setw(6) << std::setfill('0') << metadata.sequence - << " fd " << fd << " bytes " << metadata.planes()[0].bytesused - << std::endl; - } - - request->reuse(Request::ReuseBuffers); - if (capturing) - camera->queueRequest(request); + std::lock_guard lock(frameMutex); + done.push(request); + frameCv.notify_one(); } -int main() +// Pi libcamera RGB888 is BGR in memory; BGR888 gives true RGB for ffplay rgb24. +static void packRgb24(void *src, unsigned char *dst, int w, int h, int stride) { - std::unique_ptr cm = std::make_unique(); - cm->start(); + auto *p = static_cast(src); + for (int y = 0; y < h; ++y) + memcpy(dst + y * w * 3, p + y * stride, w * 3); +} - auto cameras = cm->cameras(); - if (cameras.empty()) { - std::cerr << "What? No cmaera?" << std::endl; - cameras.clear(); - cm->stop(); +static void uploadFrame(GLuint tex, const unsigned char *rgb, int w, int h) +{ + glBindTexture(GL_TEXTURE_2D, tex); + glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, w, h, GL_RGB, GL_UNSIGNED_BYTE, rgb); +} + +static GLuint makeProgram() +{ + const char *vs = + "attribute vec2 pos; attribute vec2 tex; varying vec2 uv;" + "void main(){uv=tex; gl_Position=vec4(pos,0.0,1.0);}"; + // Luminance diff avoids false motion from AWB/color shifts (common GLES motion trick). + const char *fs = + "precision mediump float; varying vec2 uv;" + "uniform sampler2D prevFrame, currFrame;" + "const vec3 lum = vec3(0.299, 0.587, 0.114);" + "void main(){" + " float a = dot(texture2D(prevFrame, uv).rgb, lum);" + " float b = dot(texture2D(currFrame, uv).rgb, lum);" + " float d = clamp(abs(b - a) * 4.0, 0.0, 1.0);" + " gl_FragColor = vec4(vec3(d), 1.0);" + "}"; + + auto compile = [](GLenum type, const char *src) { + GLuint s = glCreateShader(type); + glShaderSource(s, 1, &src, nullptr); + glCompileShader(s); + return s; + }; + GLuint p = glCreateProgram(); + glAttachShader(p, compile(GL_VERTEX_SHADER, vs)); + glAttachShader(p, compile(GL_FRAGMENT_SHADER, fs)); + glLinkProgram(p); + return p; +} + +int main(int argc, char **argv) +{ + Mode mode = parseMode(argc, argv); + std::signal(SIGINT, [](int) { capturing = false; }); + + auto cm = std::make_unique(); + cm->start(); + if (cm->cameras().empty()) { + std::cerr << "no camera\n"; return 1; } - camera = cm->get(cameras[0]->id()); - cameras.clear(); + camera = cm->get(cm->cameras()[0]->id()); camera->acquire(); - std::unique_ptr config = camera->generateConfiguration({StreamRole::Viewfinder}); - StreamConfiguration &streamConfig = config->at(0); - /*streamConfig.size.width = 640; - streamConfig.size.height = 480;*/ - config->validate(); + auto config = camera->generateConfiguration({ StreamRole::Viewfinder }); + StreamConfiguration &sc = config->at(0); + Size full = sc.size; + sc.size.width = (full.width / 2) & ~1u; + sc.size.height = (full.height / 2) & ~1u; + sc.pixelFormat = formats::BGR888; + sc.colorSpace = ColorSpace::Sycc; + sc.bufferCount = 4; + if (config->validate() == CameraConfiguration::Invalid || sc.pixelFormat != formats::BGR888) { + std::cerr << "BGR888 config failed\n"; + return 1; + } camera->configure(config.get()); + const int w = sc.size.width, h = sc.size.height, stride = sc.stride; + std::cerr << "mode=" << (mode == Mode::Raw ? "raw" : "motion") + << " " << w << "x" << h << " (half of " << full.toString() << ")" + << " stride=" << stride << "\n"; + + std::vector rgbFrame(w * h * 3), flipRow(w * 3); + + GLuint program = 0, prevTex = 0, currTex = 0, fbo = 0, vbo = 0; + GLint uPrev = -1, uCurr = -1, aPos = -1, aTex = -1; + bool havePrev = false; + EGLDisplay dpy = EGL_NO_DISPLAY; + EGLSurface surface = EGL_NO_SURFACE; + EGLContext ctx = EGL_NO_CONTEXT; + + if (mode == Mode::Motion) { + dpy = eglGetDisplay(EGL_DEFAULT_DISPLAY); + eglInitialize(dpy, nullptr, nullptr); + EGLint cfgAttr[] = { EGL_SURFACE_TYPE, EGL_PBUFFER_BIT, EGL_RENDERABLE_TYPE, EGL_OPENGL_ES2_BIT, + EGL_RED_SIZE, 8, EGL_GREEN_SIZE, 8, EGL_BLUE_SIZE, 8, EGL_NONE }; + EGLConfig cfg; + EGLint n; + eglChooseConfig(dpy, cfgAttr, &cfg, 1, &n); + EGLint pb[] = { EGL_WIDTH, w, EGL_HEIGHT, h, EGL_NONE }; + surface = eglCreatePbufferSurface(dpy, cfg, pb); + EGLint ctxAttr[] = { EGL_CONTEXT_CLIENT_VERSION, 2, EGL_NONE }; + ctx = eglCreateContext(dpy, cfg, EGL_NO_CONTEXT, ctxAttr); + eglMakeCurrent(dpy, surface, surface, ctx); + + auto makeTex = [&](int tw, int th) { + GLuint t; + glGenTextures(1, &t); + glBindTexture(GL_TEXTURE_2D, t); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, tw, th, 0, GL_RGB, GL_UNSIGNED_BYTE, nullptr); + return t; + }; + program = makeProgram(); + prevTex = makeTex(w, h); + currTex = makeTex(w, h); + GLuint outTex; + glGenTextures(1, &outTex); + glBindTexture(GL_TEXTURE_2D, outTex); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, w, h, 0, GL_RGBA, GL_UNSIGNED_BYTE, nullptr); + glGenFramebuffers(1, &fbo); + glBindFramebuffer(GL_FRAMEBUFFER, fbo); + glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, outTex, 0); + + float quad[] = { -1,-1, 0,1, 1,-1, 1,1, -1,1, 0,0, 1,1, 1,0 }; + glGenBuffers(1, &vbo); + glBindBuffer(GL_ARRAY_BUFFER, vbo); + glBufferData(GL_ARRAY_BUFFER, sizeof(quad), quad, GL_STATIC_DRAW); + + uPrev = glGetUniformLocation(program, "prevFrame"); + uCurr = glGetUniformLocation(program, "currFrame"); + aPos = glGetAttribLocation(program, "pos"); + aTex = glGetAttribLocation(program, "tex"); + } + + clientFd = waitForClient(5000, mode == Mode::Raw ? "raw rgb24" : "motion rgb24"); + FrameBufferAllocator *allocator = new FrameBufferAllocator(camera); - Stream *stream = streamConfig.stream(); + Stream *stream = sc.stream(); if (allocator->allocate(stream) < 0) { - std::cerr << "Can't allocate buffers" << std::endl; - delete allocator; - camera->release(); - cm->stop(); + std::cerr << "buffer alloc failed\n"; return 1; } - const std::vector> &buffers = allocator->buffers(stream); + for (const auto &buffer : allocator->buffers(stream)) { + const auto &plane = buffer->planes()[0]; + mapped[buffer.get()] = { + mmap(nullptr, plane.length, PROT_READ, MAP_SHARED, plane.fd.get(), plane.offset), + plane.length + }; + } + std::vector> requests; - for (const std::unique_ptr &buffer : buffers) { - std::unique_ptr request = camera->createRequest(); - if (!request || request->addBuffer(stream, buffer.get()) < 0) { - std::cerr << "Can't create request" << std::endl; - allocator->free(stream); - delete allocator; - camera->release(); - cm->stop(); + for (const auto &buffer : allocator->buffers(stream)) { + auto request = camera->createRequest(); + if (!request || request->addBuffer(stream, buffer.get()) < 0) return 1; - } requests.push_back(std::move(request)); } camera->requestCompleted.connect(requestComplete); camera->start(); - for (auto &req : requests) { - camera->queueRequest(req.get()); + for (auto &r : requests) + camera->queueRequest(r.get()); + + while (capturing) { + Request *request = nullptr; + { + std::unique_lock lock(frameMutex); + frameCv.wait_for(lock, std::chrono::milliseconds(50), [] { return !done.empty(); }); + if (done.empty()) + continue; + request = done.front(); + done.pop(); + } + + FrameBuffer *buffer = request->buffers().begin()->second; + if (buffer->metadata().status == FrameMetadata::FrameSuccess) { + void *mem = mapped[buffer].mem; + + if (mode == Mode::Raw) { + packRgb24(mem, rgbFrame.data(), w, h, stride); + if (!sendAll(rgbFrame.data(), rgbFrame.size())) + capturing = false; + } else { + packRgb24(mem, rgbFrame.data(), w, h, stride); + uploadFrame(currTex, rgbFrame.data(), w, h); + if (!havePrev) { + uploadFrame(prevTex, rgbFrame.data(), w, h); + havePrev = true; + } else { + glViewport(0, 0, w, h); + glUseProgram(program); + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D, prevTex); + glUniform1i(uPrev, 0); + glActiveTexture(GL_TEXTURE1); + glBindTexture(GL_TEXTURE_2D, currTex); + glUniform1i(uCurr, 1); + + glBindBuffer(GL_ARRAY_BUFFER, vbo); + glEnableVertexAttribArray(aPos); + glEnableVertexAttribArray(aTex); + glVertexAttribPointer(aPos, 2, GL_FLOAT, GL_FALSE, 4 * sizeof(float), (void *)0); + glVertexAttribPointer(aTex, 2, GL_FLOAT, GL_FALSE, 4 * sizeof(float), (void *)(2 * sizeof(float))); + glDrawArrays(GL_TRIANGLE_STRIP, 0, 4); + + glReadPixels(0, 0, w, h, GL_RGB, GL_UNSIGNED_BYTE, rgbFrame.data()); + for (int y = 0; y < h / 2; ++y) { + unsigned char *top = rgbFrame.data() + y * w * 3; + unsigned char *bot = rgbFrame.data() + (h - 1 - y) * w * 3; + memcpy(flipRow.data(), top, w * 3); + memcpy(top, bot, w * 3); + memcpy(bot, flipRow.data(), w * 3); + } + if (!sendAll(rgbFrame.data(), rgbFrame.size())) + capturing = false; + std::swap(prevTex, currTex); + } + } + } + + request->reuse(Request::ReuseBuffers); + camera->queueRequest(request); } - std::this_thread::sleep_for(10000ms); - - // end the cameras - capturing = false; camera->stop(); camera->requestCompleted.disconnect(); - requests.clear(); - allocator->free(streamConfig.stream()); + if (clientFd >= 0) + close(clientFd); + for (auto &[_, m] : mapped) + munmap(m.mem, m.len); + allocator->free(stream); delete allocator; + if (mode == Mode::Motion) { + eglDestroyContext(dpy, ctx); + eglDestroySurface(dpy, surface); + eglTerminate(dpy); + } camera->release(); - camera.reset(); cm->stop(); - cm.reset(); - return 0; }