Vehicle Researcher
7 years ago
parent
ab99390ec2
commit
e41a943dd0
99 changed files with 1593 additions and 2086 deletions
@ -1,3 +1,3 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
version https://git-lfs.github.com/spec/v1 |
||||||
oid sha256:51e6a2c14fae295109abb8bdeef0e8c68c36cac5d0480565c6d79fbee2dd5b16 |
oid sha256:db186a9f1e9d1564a6f82cf294eb1322224e4956a6781bfcd6e6acf40dcd92a8 |
||||||
size 2118013 |
size 2126976 |
||||||
|
|||||||
@ -1,3 +1,3 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
version https://git-lfs.github.com/spec/v1 |
||||||
oid sha256:9a080ef24391a2e93300e65cb4d38702a3fb634e30f0a48684522a73ab420fb3 |
oid sha256:dd7b7e0a5c84bef76d7f52be9dcf2641ee4ab966901deeff7228f638e112cbe6 |
||||||
size 17456889 |
size 18180759 |
||||||
|
|||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:0795c5ce6068f7ab3fcf23dccbee8b9c76da86808eacf4814ea99be4057ee83f |
|
||||||
size 12 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:ce98217d60516c28fd68135a87144be86c63bd8f5e87de64f945925b6cce46af |
|
||||||
size 1947345 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:9e392f4445062fb17266cc260288855ef9e22226910fb00a8d83eea6899c1578 |
|
||||||
size 3877605 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:01c13f193fc7e4933bcb99b4f591aaa36881197f056d3d8742c551212159e0d1 |
|
||||||
size 3521 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:8358afec7442e90c4eeffc4db4b4b00793df0f51c909976b4e95e3c8f8dacdcd |
|
||||||
size 1310659 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:ca197862754835da4f0bc30823845e3448334f8c5e2093a1947348d2fed753a2 |
|
||||||
size 176 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:b2e32152e7972032fd64ad351323f50008602c60feb19522b1aadd414bf1f7b4 |
|
||||||
size 1001 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:a07ce769d17334f803a499c520d4a7d88478ceb1a9cf740faf296f8f54616e82 |
|
||||||
size 633 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:5743dd91db33116bd7444d7407eb14b140ab2694f8525fc34c66d22256f50634 |
|
||||||
size 542612 |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:123549475cad6ae6ce2261f81caf8f094b3b3d31076e3c320fd08301eb45e657 |
|
||||||
size 194 |
|
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:c9855edf295958587122a98b881caa4fdfa2c984c8164d95cd7df58153c7304e |
||||||
|
size 12842 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:2d158f189ca1a26c9c99635d17046826ca72d050491504c48b53b10b8e8e747d |
||||||
|
size 5688 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:1fae6686e23d15a017feac32090724c18dd4c830c354ffbd1841ea57acf08ebd |
||||||
|
size 74542 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:721e3bfb6a09be9104f733e8813c6609636999b86b7952f4d7cb3032baa74e3c |
||||||
|
size 5122 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:dfbd4aa7f6a84e587c986a0b893fde1a06f1534a0643a6d140be7e94df6a45c0 |
||||||
|
size 5116 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:2c8661e6fb6fb43123c8e714c021d7741bd6de2db99ab82d50c255294064a321 |
||||||
|
size 5421 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:d9acb9d0766263b0cba5dc6136f01ee72df9c3a526125790b5022ce23183d70a |
||||||
|
size 5358 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:ae090fc06ae716732a11d886dcf1a679af55a96bfe88c73e4545cae756ddec69 |
||||||
|
size 16402 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:130faf7c95336d36c178db95ea211b9f23c6954233e144d1f7de22b95290647d |
||||||
|
size 7583 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:231b00744377552b16adf7aaa0580adf2482c0b978b9224b34470606ed0c77cf |
||||||
|
size 2869 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:dfd4ceeec4ef2b760cfac2e974a4aa6bd1e3b86bba80810c5df9d4084fdfc0b9 |
||||||
|
size 43544 |
||||||
@ -0,0 +1,3 @@ |
|||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:87140d80e5d9e1fdd9a0ba4769d4ba54e6eeadca909a4ac542bf87ec49098249 |
||||||
|
size 1993 |
||||||
@ -1 +1 @@ |
|||||||
#define COMMA_VERSION "0.4.7.2-release" |
#define COMMA_VERSION "0.5-release" |
||||||
|
|||||||
@ -0,0 +1,39 @@ |
|||||||
|
#ifndef IONBUF_H |
||||||
|
#define IONBUF_H |
||||||
|
|
||||||
|
#ifdef __APPLE__ |
||||||
|
#include <OpenCL/cl.h> |
||||||
|
#else |
||||||
|
#include <CL/cl.h> |
||||||
|
#endif |
||||||
|
|
||||||
|
#ifdef __cplusplus |
||||||
|
extern "C" { |
||||||
|
#endif |
||||||
|
|
||||||
|
typedef struct VisionBuf { |
||||||
|
size_t len; |
||||||
|
void* addr; |
||||||
|
int handle; |
||||||
|
int fd; |
||||||
|
|
||||||
|
cl_context ctx; |
||||||
|
cl_device_id device_id; |
||||||
|
cl_mem buf_cl; |
||||||
|
cl_command_queue copy_q; |
||||||
|
} VisionBuf; |
||||||
|
|
||||||
|
#define VISIONBUF_SYNC_FROM_DEVICE 0 |
||||||
|
#define VISIONBUF_SYNC_TO_DEVICE 1 |
||||||
|
|
||||||
|
VisionBuf visionbuf_allocate(size_t len); |
||||||
|
VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx, cl_mem *out_mem); |
||||||
|
cl_mem visionbuf_to_cl(const VisionBuf* buf, cl_device_id device_id, cl_context ctx); |
||||||
|
void visionbuf_sync(const VisionBuf* buf, int dir); |
||||||
|
void visionbuf_free(const VisionBuf* buf); |
||||||
|
|
||||||
|
#ifdef __cplusplus |
||||||
|
} |
||||||
|
#endif |
||||||
|
|
||||||
|
#endif |
||||||
@ -0,0 +1,141 @@ |
|||||||
|
#include <stdlib.h> |
||||||
|
#include <stdio.h> |
||||||
|
#include <assert.h> |
||||||
|
#include <sys/mman.h> |
||||||
|
#include <sys/ioctl.h> |
||||||
|
|
||||||
|
#include <linux/ion.h> |
||||||
|
#include <CL/cl_ext.h> |
||||||
|
|
||||||
|
#include <msm_ion.h> |
||||||
|
|
||||||
|
#include "visionbuf.h" |
||||||
|
|
||||||
|
|
||||||
|
// just hard-code these for convenience
|
||||||
|
// size_t device_page_size = 0;
|
||||||
|
// clGetDeviceInfo(device_id, CL_DEVICE_PAGE_SIZE_QCOM,
|
||||||
|
// sizeof(device_page_size), &device_page_size,
|
||||||
|
// NULL);
|
||||||
|
|
||||||
|
// size_t padding_cl = 0;
|
||||||
|
// clGetDeviceInfo(device_id, CL_DEVICE_EXT_MEM_PADDING_IN_BYTES_QCOM,
|
||||||
|
// sizeof(padding_cl), &padding_cl,
|
||||||
|
// NULL);
|
||||||
|
#define DEVICE_PAGE_SIZE_CL 4096 |
||||||
|
#define PADDING_CL 0 |
||||||
|
|
||||||
|
static int ion_fd = -1; |
||||||
|
static void ion_init() { |
||||||
|
if (ion_fd == -1) { |
||||||
|
ion_fd = open("/dev/ion", O_RDWR | O_NONBLOCK); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
VisionBuf visionbuf_allocate(size_t len) { |
||||||
|
int err; |
||||||
|
|
||||||
|
ion_init(); |
||||||
|
|
||||||
|
struct ion_allocation_data ion_alloc = {0}; |
||||||
|
ion_alloc.len = len + PADDING_CL; |
||||||
|
ion_alloc.align = 4096; |
||||||
|
ion_alloc.heap_id_mask = 1 << ION_IOMMU_HEAP_ID; |
||||||
|
ion_alloc.flags = ION_FLAG_CACHED; |
||||||
|
|
||||||
|
err = ioctl(ion_fd, ION_IOC_ALLOC, &ion_alloc); |
||||||
|
assert(err == 0); |
||||||
|
|
||||||
|
struct ion_fd_data ion_fd_data = {0}; |
||||||
|
ion_fd_data.handle = ion_alloc.handle; |
||||||
|
err = ioctl(ion_fd, ION_IOC_SHARE, &ion_fd_data); |
||||||
|
assert(err == 0); |
||||||
|
|
||||||
|
void *addr = mmap(NULL, ion_alloc.len, |
||||||
|
PROT_READ | PROT_WRITE, |
||||||
|
MAP_SHARED, ion_fd_data.fd, 0); |
||||||
|
assert(addr != MAP_FAILED); |
||||||
|
|
||||||
|
memset(addr, 0, ion_alloc.len); |
||||||
|
|
||||||
|
return (VisionBuf){ |
||||||
|
.len = len, |
||||||
|
.addr = addr, |
||||||
|
.handle = ion_alloc.handle, |
||||||
|
.fd = ion_fd_data.fd, |
||||||
|
}; |
||||||
|
} |
||||||
|
|
||||||
|
VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx, cl_mem *out_mem) { |
||||||
|
VisionBuf r = visionbuf_allocate(len); |
||||||
|
*out_mem = visionbuf_to_cl(&r, device_id, ctx); |
||||||
|
return r; |
||||||
|
} |
||||||
|
|
||||||
|
cl_mem visionbuf_to_cl(const VisionBuf* buf, cl_device_id device_id, cl_context ctx) { |
||||||
|
int err = 0; |
||||||
|
|
||||||
|
assert(((uintptr_t)buf->addr % DEVICE_PAGE_SIZE_CL) == 0); |
||||||
|
|
||||||
|
cl_mem_ion_host_ptr ion_cl = {0}; |
||||||
|
ion_cl.ext_host_ptr.allocation_type = CL_MEM_ION_HOST_PTR_QCOM; |
||||||
|
ion_cl.ext_host_ptr.host_cache_policy = CL_MEM_HOST_UNCACHED_QCOM; |
||||||
|
ion_cl.ion_filedesc = buf->fd; |
||||||
|
ion_cl.ion_hostptr = buf->addr; |
||||||
|
|
||||||
|
cl_mem mem = clCreateBuffer(ctx, |
||||||
|
CL_MEM_USE_HOST_PTR | CL_MEM_EXT_HOST_PTR_QCOM, |
||||||
|
buf->len, &ion_cl, &err); |
||||||
|
assert(err == 0); |
||||||
|
|
||||||
|
return mem; |
||||||
|
} |
||||||
|
|
||||||
|
void visionbuf_sync(const VisionBuf* buf, int dir) { |
||||||
|
int err; |
||||||
|
|
||||||
|
struct ion_fd_data fd_data = {0}; |
||||||
|
fd_data.fd = buf->fd; |
||||||
|
err = ioctl(ion_fd, ION_IOC_IMPORT, &fd_data); |
||||||
|
assert(err == 0); |
||||||
|
|
||||||
|
struct ion_flush_data flush_data = {0}; |
||||||
|
flush_data.handle = fd_data.handle; |
||||||
|
flush_data.vaddr = buf->addr; |
||||||
|
flush_data.offset = 0; |
||||||
|
flush_data.length = buf->len; |
||||||
|
|
||||||
|
// ION_IOC_INV_CACHES ~= DMA_FROM_DEVICE
|
||||||
|
// ION_IOC_CLEAN_CACHES ~= DMA_TO_DEVICE
|
||||||
|
// ION_IOC_CLEAN_INV_CACHES ~= DMA_BIDIRECTIONAL
|
||||||
|
|
||||||
|
struct ion_custom_data custom_data = {0}; |
||||||
|
|
||||||
|
switch (dir) { |
||||||
|
case VISIONBUF_SYNC_FROM_DEVICE: |
||||||
|
custom_data.cmd = ION_IOC_INV_CACHES; |
||||||
|
break; |
||||||
|
case VISIONBUF_SYNC_TO_DEVICE: |
||||||
|
custom_data.cmd = ION_IOC_CLEAN_CACHES; |
||||||
|
break; |
||||||
|
default: |
||||||
|
assert(0); |
||||||
|
} |
||||||
|
|
||||||
|
custom_data.arg = (unsigned long)&flush_data; |
||||||
|
err = ioctl(ion_fd, ION_IOC_CUSTOM, &custom_data); |
||||||
|
assert(err == 0); |
||||||
|
|
||||||
|
struct ion_handle_data handle_data = {0}; |
||||||
|
handle_data.handle = fd_data.handle; |
||||||
|
err = ioctl(ion_fd, ION_IOC_FREE, &handle_data); |
||||||
|
assert(err == 0); |
||||||
|
} |
||||||
|
|
||||||
|
void visionbuf_free(const VisionBuf* buf) { |
||||||
|
struct ion_handle_data handle_data = { |
||||||
|
.handle = buf->handle, |
||||||
|
}; |
||||||
|
int ret = ioctl(ion_fd, ION_IOC_FREE, &handle_data); |
||||||
|
assert(ret == 0); |
||||||
|
} |
||||||
@ -0,0 +1,111 @@ |
|||||||
|
#include <cassert> |
||||||
|
|
||||||
|
#ifdef QCOM |
||||||
|
#include <system/graphics.h> |
||||||
|
#include <ui/GraphicBuffer.h> |
||||||
|
#include <ui/PixelFormat.h> |
||||||
|
#include <gralloc_priv.h> |
||||||
|
|
||||||
|
#include <GLES3/gl3.h> |
||||||
|
#define GL_GLEXT_PROTOTYPES |
||||||
|
#include <GLES2/gl2ext.h> |
||||||
|
|
||||||
|
#include <EGL/egl.h> |
||||||
|
#define EGL_EGLEXT_PROTOTYPES |
||||||
|
#include <EGL/eglext.h> |
||||||
|
|
||||||
|
#endif |
||||||
|
|
||||||
|
#include "common/util.h" |
||||||
|
#include "common/visionbuf.h" |
||||||
|
|
||||||
|
#include "common/visionimg.h" |
||||||
|
|
||||||
|
#ifdef QCOM |
||||||
|
|
||||||
|
using namespace android; |
||||||
|
|
||||||
|
// from libadreno_utils.so
|
||||||
|
extern "C" void compute_aligned_width_and_height(int width, |
||||||
|
int height, |
||||||
|
int bpp, |
||||||
|
int tile_mode, |
||||||
|
int raster_mode, |
||||||
|
int padding_threshold, |
||||||
|
int *aligned_w, |
||||||
|
int *aligned_h); |
||||||
|
#endif |
||||||
|
|
||||||
|
VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf) { |
||||||
|
|
||||||
|
int aligned_w = 0, aligned_h = 0; |
||||||
|
#ifdef QCOM |
||||||
|
compute_aligned_width_and_height(ALIGN(width, 32), ALIGN(height, 32), 3, 0, 0, 512, &aligned_w, &aligned_h); |
||||||
|
#else |
||||||
|
aligned_w = width; aligned_h = height; |
||||||
|
#endif |
||||||
|
|
||||||
|
int stride = aligned_w * 3; |
||||||
|
size_t size = aligned_w * aligned_h * 3; |
||||||
|
|
||||||
|
VisionBuf buf = visionbuf_allocate(size); |
||||||
|
|
||||||
|
*out_buf = buf; |
||||||
|
|
||||||
|
return (VisionImg){ |
||||||
|
.fd = buf.fd, |
||||||
|
.format = VISIONIMG_FORMAT_RGB24, |
||||||
|
.width = width, |
||||||
|
.height = height, |
||||||
|
.stride = stride, |
||||||
|
.size = size, |
||||||
|
.bpp = 3, |
||||||
|
}; |
||||||
|
} |
||||||
|
|
||||||
|
#ifdef QCOM |
||||||
|
|
||||||
|
EGLClientBuffer visionimg_to_egl(const VisionImg *img) { |
||||||
|
assert((img->size % img->stride) == 0); |
||||||
|
assert((img->stride % img->bpp) == 0); |
||||||
|
|
||||||
|
int format = 0; |
||||||
|
if (img->format == VISIONIMG_FORMAT_RGB24) { |
||||||
|
format = HAL_PIXEL_FORMAT_RGB_888; |
||||||
|
} else { |
||||||
|
assert(false); |
||||||
|
} |
||||||
|
|
||||||
|
private_handle_t* hnd = new private_handle_t(img->fd, img->size, |
||||||
|
private_handle_t::PRIV_FLAGS_USES_ION|private_handle_t::PRIV_FLAGS_FRAMEBUFFER, |
||||||
|
0, format, |
||||||
|
img->stride/img->bpp, img->size/img->stride, |
||||||
|
img->width, img->height); |
||||||
|
|
||||||
|
GraphicBuffer* gb = new GraphicBuffer(img->width, img->height, (PixelFormat)format, |
||||||
|
GraphicBuffer::USAGE_HW_TEXTURE, img->stride/img->bpp, hnd, false); |
||||||
|
|
||||||
|
return (EGLClientBuffer) gb->getNativeBuffer(); |
||||||
|
} |
||||||
|
|
||||||
|
GLuint visionimg_to_gl(const VisionImg *img) { |
||||||
|
|
||||||
|
EGLClientBuffer buf = visionimg_to_egl(img); |
||||||
|
|
||||||
|
EGLDisplay display = eglGetDisplay(EGL_DEFAULT_DISPLAY); |
||||||
|
assert(display != EGL_NO_DISPLAY); |
||||||
|
|
||||||
|
EGLint img_attrs[] = { EGL_IMAGE_PRESERVED_KHR, EGL_TRUE, EGL_NONE }; |
||||||
|
EGLImageKHR image = eglCreateImageKHR(display, EGL_NO_CONTEXT, |
||||||
|
EGL_NATIVE_BUFFER_ANDROID, buf, img_attrs); |
||||||
|
assert(image != EGL_NO_IMAGE_KHR); |
||||||
|
|
||||||
|
GLuint tex = 0; |
||||||
|
glGenTextures(1, &tex); |
||||||
|
glBindTexture(GL_TEXTURE_2D, tex); |
||||||
|
glEGLImageTargetTexture2DOES(GL_TEXTURE_2D, image); |
||||||
|
|
||||||
|
return tex; |
||||||
|
} |
||||||
|
|
||||||
|
#endif |
||||||
@ -0,0 +1,37 @@ |
|||||||
|
#ifndef VISIONIMG_H |
||||||
|
#define VISIONIMG_H |
||||||
|
|
||||||
|
#ifdef QCOM |
||||||
|
#include <GLES3/gl3.h> |
||||||
|
#include <EGL/egl.h> |
||||||
|
#include <EGL/eglext.h> |
||||||
|
#endif |
||||||
|
|
||||||
|
#include "common/visionbuf.h" |
||||||
|
|
||||||
|
#ifdef __cplusplus |
||||||
|
extern "C" { |
||||||
|
#endif |
||||||
|
|
||||||
|
#define VISIONIMG_FORMAT_RGB24 1 |
||||||
|
|
||||||
|
typedef struct VisionImg { |
||||||
|
int fd; |
||||||
|
int format; |
||||||
|
int width, height, stride; |
||||||
|
int bpp; |
||||||
|
size_t size; |
||||||
|
} VisionImg; |
||||||
|
|
||||||
|
VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf); |
||||||
|
|
||||||
|
#ifdef QCOM |
||||||
|
EGLClientBuffer visionimg_to_egl(const VisionImg *img); |
||||||
|
GLuint visionimg_to_gl(const VisionImg *img); |
||||||
|
#endif |
||||||
|
|
||||||
|
#ifdef __cplusplus |
||||||
|
} // extern "C"
|
||||||
|
#endif |
||||||
|
|
||||||
|
#endif |
||||||
@ -0,0 +1,121 @@ |
|||||||
|
import numpy as np |
||||||
|
from common.realtime import sec_since_boot |
||||||
|
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET |
||||||
|
|
||||||
|
_DT = 0.01 # update runs at 100Hz |
||||||
|
_AWARENESS_TIME = 180 # 3 minutes limit without user touching steering wheels make the car enter a terminal status |
||||||
|
_AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before expiration |
||||||
|
_AWARENESS_PROMPT_TIME = 5. # a second alert is issued 5s before start decelerating the car |
||||||
|
_DISTRACTED_TIME = 6. |
||||||
|
_DISTRACTED_PRE_TIME = 4. |
||||||
|
_DISTRACTED_PROMPT_TIME = 2. |
||||||
|
# measured 1 rad in x FOV. 1152x864 is original image, 160x320 is a right crop for model |
||||||
|
_CAMERA_FOV_X = 1. # rad |
||||||
|
_CAMERA_FOV_Y = 0.75 # 4/3 aspect ratio |
||||||
|
# model output refers to center of cropped image, so need to apply the x displacement offset |
||||||
|
_CAMERA_OFFSET_X = 0.3125 #(1152/2 - 0.5*(160*864/320))/1152 |
||||||
|
_CAMERA_X_CONV = 0.375 # 160*864/320/1152 |
||||||
|
_PITCH_WEIGHT = 1.5 # pitch matters a lot more |
||||||
|
_METRIC_THRESHOLD = 0.4 |
||||||
|
_PITCH_POS_ALLOWANCE = 0.08 # rad, to not be too sensitive on positive pitch |
||||||
|
_DTM = 0.2 # driver monitor runs at 5Hz |
||||||
|
_DISTRACTED_FILTER_F = 0.3 # 0.3Hz |
||||||
|
_DISTRACTED_FILTER_K = 2 * np.pi * _DISTRACTED_FILTER_F * _DTM / (1 + 2 * np.pi * _DISTRACTED_FILTER_F * _DTM) |
||||||
|
_PITCH_NATURAL_OFFSET = 0.1 # people don't seem to look straight when they drive relaxed, rather a bit up |
||||||
|
|
||||||
|
|
||||||
|
class _DriverPose(): |
||||||
|
def __init__(self): |
||||||
|
self.yaw = 0. |
||||||
|
self.pitch = 0. |
||||||
|
self.roll = 0. |
||||||
|
self.yaw_offset = 0. |
||||||
|
self.pitch_offset = 0. |
||||||
|
|
||||||
|
class DriverStatus(): |
||||||
|
def __init__(self, monitor_on): |
||||||
|
self.pose = _DriverPose() |
||||||
|
self.monitor_on = monitor_on |
||||||
|
self.awareness = 1. |
||||||
|
self.driver_distracted = False |
||||||
|
self.driver_distraction_level = 0. |
||||||
|
self.ts_last_check = 0. |
||||||
|
self._set_timers() |
||||||
|
|
||||||
|
def _set_timers(self): |
||||||
|
if self.monitor_on: |
||||||
|
self.threshold_pre = _DISTRACTED_PRE_TIME / _DISTRACTED_TIME |
||||||
|
self.threshold_prompt = _DISTRACTED_PROMPT_TIME / _DISTRACTED_TIME |
||||||
|
self.step_change = _DT / _DISTRACTED_TIME |
||||||
|
else: |
||||||
|
self.threshold_pre = _AWARENESS_PRE_TIME / _AWARENESS_TIME |
||||||
|
self.threshold_prompt = _AWARENESS_PROMPT_TIME / _AWARENESS_TIME |
||||||
|
self.step_change = _DT / _AWARENESS_TIME |
||||||
|
|
||||||
|
def _is_driver_distracted(self, pose): |
||||||
|
# to be tuned and to learn the driver's normal pose |
||||||
|
yaw_error = pose.yaw - pose.yaw_offset |
||||||
|
pitch_error = pose.pitch - pose.pitch_offset - _PITCH_NATURAL_OFFSET |
||||||
|
# add positive pitch allowance |
||||||
|
if pitch_error > 0.: |
||||||
|
pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.) |
||||||
|
pitch_error *= _PITCH_WEIGHT |
||||||
|
metric = np.sqrt(yaw_error**2 + pitch_error**2) |
||||||
|
#print "%02.4f" % np.degrees(pose.pitch), "%02.4f" % np.degrees(pitch_error), "%03.4f" % np.degrees(pose.pitch_offset), metric |
||||||
|
return 1 if metric > _METRIC_THRESHOLD else 0 |
||||||
|
|
||||||
|
def get_pose(self, driver_monitoring, params): |
||||||
|
ts = sec_since_boot() |
||||||
|
|
||||||
|
# don's check for param too often as it's a kernel call |
||||||
|
if ts - self.ts_last_check > 1.: |
||||||
|
self.monitor_on = params.get("IsDriverMonitoringEnabled") == "1" |
||||||
|
self._set_timers() |
||||||
|
self.ts_last_check = ts |
||||||
|
|
||||||
|
self.pose.pitch = driver_monitoring.descriptor[0] |
||||||
|
self.pose.yaw = driver_monitoring.descriptor[1] |
||||||
|
self.pose.roll = driver_monitoring.descriptor[2] |
||||||
|
self.pose.yaw_offset = (driver_monitoring.descriptor[3] * _CAMERA_X_CONV + _CAMERA_OFFSET_X) * _CAMERA_FOV_X |
||||||
|
self.pose.pitch_offset = -driver_monitoring.descriptor[4] * _CAMERA_FOV_Y # positive y is down |
||||||
|
self.driver_distracted = self._is_driver_distracted(self.pose) |
||||||
|
# first order filter |
||||||
|
self.driver_distraction_level = (1. - _DISTRACTED_FILTER_K) * self.driver_distraction_level + \ |
||||||
|
_DISTRACTED_FILTER_K * self.driver_distracted |
||||||
|
|
||||||
|
def update(self, events, driver_engaged, ctrl_active, standstill): |
||||||
|
|
||||||
|
driver_engaged |= (self.driver_distraction_level < 0.37 and self.monitor_on) |
||||||
|
|
||||||
|
if (driver_engaged and self.awareness > 0.) or not ctrl_active: |
||||||
|
# always reset if driver is in control (unless we are in red alert state) or op isn't active |
||||||
|
self.awareness = 1. |
||||||
|
|
||||||
|
if (not self.monitor_on or (self.driver_distraction_level > 0.63 and self.driver_distracted)) and \ |
||||||
|
not (standstill and self.awareness - self.step_change <= self.threshold_prompt): |
||||||
|
self.awareness = max(self.awareness - self.step_change, -0.1) |
||||||
|
|
||||||
|
if self.awareness <= 0.: |
||||||
|
# terminal red alert: disengagement required |
||||||
|
events.append(create_event('driverDistracted', [ET.WARNING])) |
||||||
|
elif self.awareness <= self.threshold_prompt: |
||||||
|
# prompt orange alert |
||||||
|
events.append(create_event('promptDriverDistracted', [ET.WARNING])) |
||||||
|
elif self.awareness <= self.threshold_pre: |
||||||
|
# pre green alert |
||||||
|
events.append(create_event('preDriverDistracted', [ET.WARNING])) |
||||||
|
|
||||||
|
return events |
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
ds = DriverStatus(True) |
||||||
|
ds.driver_distraction_level = 1. |
||||||
|
ds.driver_distracted = 1 |
||||||
|
for i in range(1000): |
||||||
|
ds.update([], False, True, True) |
||||||
|
print(ds.awareness, ds.driver_distracted, ds.driver_distraction_level) |
||||||
|
ds.update([], True, True, False) |
||||||
|
print(ds.awareness, ds.driver_distracted, ds.driver_distraction_level) |
||||||
|
|
||||||
|
|
||||||
@ -1,73 +0,0 @@ |
|||||||
#!/usr/bin/env python |
|
||||||
import zmq |
|
||||||
from copy import copy |
|
||||||
from selfdrive import messaging |
|
||||||
from selfdrive.services import service_list |
|
||||||
from cereal import log |
|
||||||
|
|
||||||
from common.transformations.coordinates import geodetic2ecef |
|
||||||
|
|
||||||
def main(gctx=None): |
|
||||||
context = zmq.Context() |
|
||||||
poller = zmq.Poller() |
|
||||||
gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, poller) |
|
||||||
gps_ext_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, poller) |
|
||||||
app_sock = messaging.sub_sock(context, service_list['applanixLocation'].port, poller) |
|
||||||
loc_sock = messaging.pub_sock(context, service_list['liveLocation'].port) |
|
||||||
|
|
||||||
last_ext, last_gps, last_app = -1, -1, -1 |
|
||||||
# 5 sec |
|
||||||
max_gap = 5*1e9 |
|
||||||
preferred_type = None |
|
||||||
|
|
||||||
while 1: |
|
||||||
for sock, event in poller.poll(500): |
|
||||||
if sock is app_sock: |
|
||||||
msg = messaging.recv_one(sock) |
|
||||||
last_app = msg.logMonoTime |
|
||||||
this_type = 'app' |
|
||||||
if sock is gps_sock: |
|
||||||
msg = messaging.recv_one(sock) |
|
||||||
gps_pkt = msg.gpsLocation |
|
||||||
last_gps = msg.logMonoTime |
|
||||||
this_type = 'gps' |
|
||||||
if sock is gps_ext_sock: |
|
||||||
msg = messaging.recv_one(sock) |
|
||||||
gps_pkt = msg.gpsLocationExternal |
|
||||||
last_ext = msg.logMonoTime |
|
||||||
this_type = 'ext' |
|
||||||
|
|
||||||
last = max(last_gps, last_ext, last_app) |
|
||||||
|
|
||||||
if last_app > last - max_gap: |
|
||||||
new_preferred_type = 'app' |
|
||||||
elif last_ext > last - max_gap: |
|
||||||
new_preferred_type = 'ext' |
|
||||||
else: |
|
||||||
new_preferred_type = 'gps' |
|
||||||
|
|
||||||
if preferred_type != new_preferred_type: |
|
||||||
print "switching from %s to %s" % (preferred_type, new_preferred_type) |
|
||||||
preferred_type = new_preferred_type |
|
||||||
|
|
||||||
if this_type == preferred_type: |
|
||||||
new_msg = messaging.new_message() |
|
||||||
if this_type == 'app': |
|
||||||
# straight proxy the applanix |
|
||||||
new_msg.init('liveLocation') |
|
||||||
new_msg.liveLocation = copy(msg.applanixLocation) |
|
||||||
else: |
|
||||||
new_msg.logMonoTime = msg.logMonoTime |
|
||||||
new_msg.init('liveLocation') |
|
||||||
pkt = new_msg.liveLocation |
|
||||||
pkt.lat = gps_pkt.latitude |
|
||||||
pkt.lon = gps_pkt.longitude |
|
||||||
pkt.alt = gps_pkt.altitude |
|
||||||
pkt.speed = gps_pkt.speed |
|
||||||
pkt.heading = gps_pkt.bearing |
|
||||||
pkt.positionECEF = [float(x) for x in geodetic2ecef([pkt.lat, pkt.lon, pkt.alt])] |
|
||||||
pkt.source = log.LiveLocationData.SensorSource.dummy |
|
||||||
loc_sock.send(new_msg.to_bytes()) |
|
||||||
|
|
||||||
if __name__ == '__main__': |
|
||||||
main() |
|
||||||
@ -1,3 +1,3 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
version https://git-lfs.github.com/spec/v1 |
||||||
oid sha256:fb9ada8972a8a79dcf9af70260b9ac2ada87eea7acf680349a3581e75028587a |
oid sha256:819e9c55b925cdaa7b9f818c09c649926420fb6892f47e4234954961b9d9191c |
||||||
size 1622712 |
size 1622728 |
||||||
|
|||||||
@ -1,8 +0,0 @@ |
|||||||
orbd |
|
||||||
orbd_cpu |
|
||||||
test/turbocv_profile |
|
||||||
test/turbocv_test |
|
||||||
dspout/* |
|
||||||
dumb_test |
|
||||||
bilinear_lut.h |
|
||||||
orb_lut.h |
|
||||||
@ -1,105 +0,0 @@ |
|||||||
# CPU
|
|
||||||
|
|
||||||
CC = clang
|
|
||||||
CXX = clang++
|
|
||||||
|
|
||||||
WARN_FLAGS = -Werror=implicit-function-declaration \
|
|
||||||
-Werror=incompatible-pointer-types \
|
|
||||||
-Werror=int-conversion \
|
|
||||||
-Werror=return-type \
|
|
||||||
-Werror=format-extra-args
|
|
||||||
|
|
||||||
JSON_FLAGS = -I$(PHONELIBS)/json/src
|
|
||||||
|
|
||||||
CFLAGS = -std=gnu11 -g -O2 -fPIC $(WARN_FLAGS) -Iinclude $(JSON_FLAGS) -I.
|
|
||||||
CXXFLAGS = -std=c++11 -g -O2 -fPIC $(WARN_FLAGS) -Iinclude $(JSON_FLAGS) -I.
|
|
||||||
LDFLAGS =
|
|
||||||
|
|
||||||
# profile
|
|
||||||
# CXXFLAGS += -DTURBOCV_PROFILE=1
|
|
||||||
|
|
||||||
PHONELIBS = ../../phonelibs
|
|
||||||
BASEDIR = ../..
|
|
||||||
EXTERNAL = ../../external
|
|
||||||
PYTHONLIBS =
|
|
||||||
|
|
||||||
UNAME_M := $(shell uname -m)
|
|
||||||
|
|
||||||
ifeq ($(UNAME_M),x86_64) |
|
||||||
# computer
|
|
||||||
|
|
||||||
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
|
|
||||||
ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib/ \
|
|
||||||
-l:libczmq.a -l:libzmq.a -lpthread
|
|
||||||
|
|
||||||
OPENCV_LIBS = -lopencv_core -lopencv_highgui -lopencv_features2d -lopencv_imgproc
|
|
||||||
|
|
||||||
CXXFLAGS += -fopenmp
|
|
||||||
LDFLAGS += -lomp
|
|
||||||
|
|
||||||
else |
|
||||||
# phone
|
|
||||||
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
|
|
||||||
ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
|
|
||||||
-l:libczmq.a -l:libzmq.a \
|
|
||||||
-lgnustl_shared
|
|
||||||
|
|
||||||
OPENCV_FLAGS = -I$(PHONELIBS)/opencv/include
|
|
||||||
OPENCV_LIBS = -Wl,--enable-new-dtags -Wl,-rpath,/usr/local/lib/python2.7/site-packages -L/usr/local/lib/python2.7/site-packages -l:cv2.so
|
|
||||||
|
|
||||||
endif |
|
||||||
|
|
||||||
.PHONY: all |
|
||||||
all: orbd |
|
||||||
|
|
||||||
include ../common/cereal.mk |
|
||||||
|
|
||||||
DEP_OBJS = ../common/visionipc.o ../common/swaglog.o $(PHONELIBS)/json/src/json.o
|
|
||||||
|
|
||||||
orbd: orbd_dsp.o $(DEP_OBJS) calculator_stub.o freethedsp.o |
|
||||||
@echo "[ LINK ] $@"
|
|
||||||
$(CXX) -fPIC -o '$@' $^ \
|
|
||||||
$(LDFLAGS) \
|
|
||||||
$(ZMQ_LIBS) \
|
|
||||||
$(CEREAL_LIBS) \
|
|
||||||
-L/usr/lib \
|
|
||||||
-L/system/vendor/lib64 \
|
|
||||||
-ladsprpc \
|
|
||||||
-lm -lz -llog
|
|
||||||
|
|
||||||
%.o: %.c |
|
||||||
@echo "[ CC ] $@"
|
|
||||||
$(CC) $(CFLAGS) \
|
|
||||||
$(ZMQ_FLAGS) \
|
|
||||||
-I../ \
|
|
||||||
-I../../ \
|
|
||||||
-c -o '$@' '$<'
|
|
||||||
|
|
||||||
orbd_dsp.o: orbd.cc |
|
||||||
@echo "[ CXX ] $@"
|
|
||||||
$(CXX) $(CXXFLAGS) \
|
|
||||||
$(CEREAL_CXXFLAGS) \
|
|
||||||
$(ZMQ_FLAGS) \
|
|
||||||
$(OPENCV_FLAGS) \
|
|
||||||
-DDSP \
|
|
||||||
-I../ \
|
|
||||||
-I../../ \
|
|
||||||
-I../../../ \
|
|
||||||
-I./include \
|
|
||||||
-c -o '$@' '$<'
|
|
||||||
|
|
||||||
freethedsp.o: dsp/freethedsp.c |
|
||||||
@echo "[ CC ] $@"
|
|
||||||
$(CC) $(CFLAGS) \
|
|
||||||
-c -o '$@' '$<'
|
|
||||||
|
|
||||||
calculator_stub.o: dsp/gen/calculator_stub.c |
|
||||||
@echo "[ CC ] $@"
|
|
||||||
$(CC) $(CFLAGS) -I./include -c -o '$@' '$<'
|
|
||||||
|
|
||||||
-include internal.mk |
|
||||||
|
|
||||||
.PHONY: clean |
|
||||||
clean: |
|
||||||
rm -f *.o turbocv.so orbd test/turbocv_profile test/turbocv_test test/*.o *_lut.h
|
|
||||||
|
|
||||||
@ -1,119 +0,0 @@ |
|||||||
// freethedsp by geohot
|
|
||||||
// (because the DSP should be free)
|
|
||||||
// released under MIT License
|
|
||||||
|
|
||||||
// usage instructions:
|
|
||||||
// 1. Compile an example from the Qualcomm Hexagon SDK
|
|
||||||
// 2. Try to run it on your phone
|
|
||||||
// 3. Be very sad when "adsprpc ... dlopen error: ... signature verify start failed for ..." appears in logcat
|
|
||||||
// ...here is where people would give up before freethedsp
|
|
||||||
// 4. Compile freethedsp with 'clang -shared freethedsp.c -o freethedsp.so' (or statically link it to your program)
|
|
||||||
// 5. Run your program with 'LD_PRELOAD=./freethedsp.so ./<your_prog>'
|
|
||||||
// 6. OMG THE DSP WORKS
|
|
||||||
// 7. Be happy.
|
|
||||||
|
|
||||||
// *** patch may have to change for your phone ***
|
|
||||||
|
|
||||||
// this is patching /dsp/fastrpc_shell_0
|
|
||||||
// correct if sha hash of fastrpc_shell_0 is "fbadc96848aefad99a95aa4edb560929dcdf78f8"
|
|
||||||
// patch to return 0xFFFFFFFF from is_test_enabled instead of 0
|
|
||||||
// your fastrpc_shell_0 may vary
|
|
||||||
#define PATCH_ADDR 0x5200c |
|
||||||
#define PATCH_OLD "\x40\x3f\x20\x50" |
|
||||||
#define PATCH_NEW "\x40\x3f\x00\x5a" |
|
||||||
#define PATCH_LEN (sizeof(PATCH_OLD)-1) |
|
||||||
#define _BITS_IOCTL_H_ |
|
||||||
|
|
||||||
// under 100 lines of code begins now
|
|
||||||
#include <stdio.h> |
|
||||||
#include <dlfcn.h> |
|
||||||
#include <assert.h> |
|
||||||
#include <stdlib.h> |
|
||||||
#include <unistd.h> |
|
||||||
|
|
||||||
// ioctl stuff
|
|
||||||
#define IOC_OUT 0x40000000 /* copy out parameters */ |
|
||||||
#define IOC_IN 0x80000000 /* copy in parameters */ |
|
||||||
#define IOC_INOUT (IOC_IN|IOC_OUT) |
|
||||||
#define IOCPARM_MASK 0x1fff /* parameter length, at most 13 bits */ |
|
||||||
|
|
||||||
#define _IOC(inout,group,num,len) \ |
|
||||||
(inout | ((len & IOCPARM_MASK) << 16) | ((group) << 8) | (num)) |
|
||||||
#define _IOWR(g,n,t) _IOC(IOC_INOUT, (g), (n), sizeof(t)) |
|
||||||
|
|
||||||
// ion ioctls
|
|
||||||
#include <linux/ion.h> |
|
||||||
#define ION_IOC_MSM_MAGIC 'M' |
|
||||||
#define ION_IOC_CLEAN_INV_CACHES _IOWR(ION_IOC_MSM_MAGIC, 2, \ |
|
||||||
struct ion_flush_data) |
|
||||||
|
|
||||||
struct ion_flush_data { |
|
||||||
ion_user_handle_t handle; |
|
||||||
int fd; |
|
||||||
void *vaddr; |
|
||||||
unsigned int offset; |
|
||||||
unsigned int length; |
|
||||||
}; |
|
||||||
|
|
||||||
// fastrpc ioctls
|
|
||||||
#define FASTRPC_IOCTL_INIT _IOWR('R', 6, struct fastrpc_ioctl_init) |
|
||||||
|
|
||||||
struct fastrpc_ioctl_init { |
|
||||||
uint32_t flags; /* one of FASTRPC_INIT_* macros */ |
|
||||||
uintptr_t __user file; /* pointer to elf file */ |
|
||||||
int32_t filelen; /* elf file length */ |
|
||||||
int32_t filefd; /* ION fd for the file */ |
|
||||||
uintptr_t __user mem; /* mem for the PD */ |
|
||||||
int32_t memlen; /* mem length */ |
|
||||||
int32_t memfd; /* ION fd for the mem */ |
|
||||||
}; |
|
||||||
|
|
||||||
int ioctl(int fd, unsigned long request, void *arg) { |
|
||||||
static void *handle = NULL; |
|
||||||
static int (*orig_ioctl)(int, int, void*); |
|
||||||
|
|
||||||
if (handle == NULL) { |
|
||||||
handle = dlopen("/system/lib64/libc.so", RTLD_LAZY); |
|
||||||
assert(handle != NULL); |
|
||||||
orig_ioctl = dlsym(handle, "ioctl"); |
|
||||||
} |
|
||||||
|
|
||||||
int ret = orig_ioctl(fd, request, arg); |
|
||||||
|
|
||||||
// carefully modify this one
|
|
||||||
if (request == FASTRPC_IOCTL_INIT) { |
|
||||||
struct fastrpc_ioctl_init *init = (struct fastrpc_ioctl_init *)arg; |
|
||||||
|
|
||||||
// confirm patch is correct and do the patch
|
|
||||||
assert(memcmp((void*)(init->mem+PATCH_ADDR), PATCH_OLD, PATCH_LEN) == 0); |
|
||||||
memcpy((void*)(init->mem+PATCH_ADDR), PATCH_NEW, PATCH_LEN); |
|
||||||
|
|
||||||
// flush cache
|
|
||||||
int ionfd = open("/dev/ion", O_RDONLY); |
|
||||||
assert(ionfd > 0); |
|
||||||
|
|
||||||
struct ion_fd_data fd_data; |
|
||||||
fd_data.fd = init->memfd; |
|
||||||
int ret = ioctl(ionfd, ION_IOC_IMPORT, &fd_data); |
|
||||||
assert(ret == 0); |
|
||||||
|
|
||||||
struct ion_flush_data flush_data; |
|
||||||
flush_data.handle = fd_data.handle; |
|
||||||
flush_data.vaddr = (void*)init->mem; |
|
||||||
flush_data.offset = 0; |
|
||||||
flush_data.length = init->memlen; |
|
||||||
ret = ioctl(ionfd, ION_IOC_CLEAN_INV_CACHES, &flush_data); |
|
||||||
assert(ret == 0); |
|
||||||
|
|
||||||
struct ion_handle_data handle_data; |
|
||||||
handle_data.handle = fd_data.handle; |
|
||||||
ret = ioctl(ionfd, ION_IOC_FREE, &handle_data); |
|
||||||
assert(ret == 0); |
|
||||||
|
|
||||||
// cleanup
|
|
||||||
close(ionfd); |
|
||||||
} |
|
||||||
|
|
||||||
return ret; |
|
||||||
} |
|
||||||
|
|
||||||
@ -1,39 +0,0 @@ |
|||||||
#ifndef _CALCULATOR_H |
|
||||||
#define _CALCULATOR_H |
|
||||||
|
|
||||||
#include <stdint.h> |
|
||||||
typedef uint8_t uint8; |
|
||||||
typedef uint32_t uint32; |
|
||||||
|
|
||||||
#ifndef __QAIC_HEADER |
|
||||||
#define __QAIC_HEADER(ff) ff |
|
||||||
#endif //__QAIC_HEADER
|
|
||||||
|
|
||||||
#ifndef __QAIC_HEADER_EXPORT |
|
||||||
#define __QAIC_HEADER_EXPORT |
|
||||||
#endif // __QAIC_HEADER_EXPORT
|
|
||||||
|
|
||||||
#ifndef __QAIC_HEADER_ATTRIBUTE |
|
||||||
#define __QAIC_HEADER_ATTRIBUTE |
|
||||||
#endif // __QAIC_HEADER_ATTRIBUTE
|
|
||||||
|
|
||||||
#ifndef __QAIC_IMPL |
|
||||||
#define __QAIC_IMPL(ff) ff |
|
||||||
#endif //__QAIC_IMPL
|
|
||||||
|
|
||||||
#ifndef __QAIC_IMPL_EXPORT |
|
||||||
#define __QAIC_IMPL_EXPORT |
|
||||||
#endif // __QAIC_IMPL_EXPORT
|
|
||||||
|
|
||||||
#ifndef __QAIC_IMPL_ATTRIBUTE |
|
||||||
#define __QAIC_IMPL_ATTRIBUTE |
|
||||||
#endif // __QAIC_IMPL_ATTRIBUTE
|
|
||||||
#ifdef __cplusplus |
|
||||||
extern "C" { |
|
||||||
#endif |
|
||||||
__QAIC_HEADER_EXPORT int __QAIC_HEADER(calculator_init)(uint32* leet) __QAIC_HEADER_ATTRIBUTE; |
|
||||||
__QAIC_HEADER_EXPORT int __QAIC_HEADER(calculator_extract_and_match)(const uint8* img, int imgLen, uint8* features, int featuresLen) __QAIC_HEADER_ATTRIBUTE; |
|
||||||
#ifdef __cplusplus |
|
||||||
} |
|
||||||
#endif |
|
||||||
#endif //_CALCULATOR_H
|
|
||||||
@ -1,613 +0,0 @@ |
|||||||
#ifndef _CALCULATOR_STUB_H |
|
||||||
#define _CALCULATOR_STUB_H |
|
||||||
#include "calculator.h" |
|
||||||
|
|
||||||
// remote.h
|
|
||||||
#include <stdint.h> |
|
||||||
#include <sys/types.h> |
|
||||||
|
|
||||||
typedef uint32_t remote_handle; |
|
||||||
typedef uint64_t remote_handle64; |
|
||||||
|
|
||||||
typedef struct { |
|
||||||
void *pv; |
|
||||||
size_t nLen; |
|
||||||
} remote_buf; |
|
||||||
|
|
||||||
typedef struct { |
|
||||||
int32_t fd; |
|
||||||
uint32_t offset; |
|
||||||
} remote_dma_handle; |
|
||||||
|
|
||||||
typedef union { |
|
||||||
remote_buf buf; |
|
||||||
remote_handle h; |
|
||||||
remote_handle64 h64; |
|
||||||
remote_dma_handle dma; |
|
||||||
} remote_arg; |
|
||||||
|
|
||||||
int remote_handle_open(const char* name, remote_handle *ph); |
|
||||||
int remote_handle_invoke(remote_handle h, uint32_t dwScalars, remote_arg *pra); |
|
||||||
int remote_handle_close(remote_handle h); |
|
||||||
|
|
||||||
#define REMOTE_SCALARS_MAKEX(nAttr,nMethod,nIn,nOut,noIn,noOut) \ |
|
||||||
((((uint32_t) (nAttr) & 0x7) << 29) | \
|
|
||||||
(((uint32_t) (nMethod) & 0x1f) << 24) | \
|
|
||||||
(((uint32_t) (nIn) & 0xff) << 16) | \
|
|
||||||
(((uint32_t) (nOut) & 0xff) << 8) | \
|
|
||||||
(((uint32_t) (noIn) & 0x0f) << 4) | \
|
|
||||||
((uint32_t) (noOut) & 0x0f)) |
|
||||||
|
|
||||||
#ifndef _QAIC_ENV_H |
|
||||||
#define _QAIC_ENV_H |
|
||||||
|
|
||||||
#ifdef __GNUC__ |
|
||||||
#ifdef __clang__ |
|
||||||
#pragma GCC diagnostic ignored "-Wunknown-pragmas" |
|
||||||
#else |
|
||||||
#pragma GCC diagnostic ignored "-Wpragmas" |
|
||||||
#endif |
|
||||||
#pragma GCC diagnostic ignored "-Wuninitialized" |
|
||||||
#pragma GCC diagnostic ignored "-Wunused-parameter" |
|
||||||
#pragma GCC diagnostic ignored "-Wunused-function" |
|
||||||
#endif |
|
||||||
|
|
||||||
#ifndef _ATTRIBUTE_UNUSED |
|
||||||
|
|
||||||
#ifdef _WIN32 |
|
||||||
#define _ATTRIBUTE_UNUSED |
|
||||||
#else |
|
||||||
#define _ATTRIBUTE_UNUSED __attribute__ ((unused)) |
|
||||||
#endif |
|
||||||
|
|
||||||
#endif // _ATTRIBUTE_UNUSED
|
|
||||||
|
|
||||||
#ifndef __QAIC_REMOTE |
|
||||||
#define __QAIC_REMOTE(ff) ff |
|
||||||
#endif //__QAIC_REMOTE
|
|
||||||
|
|
||||||
#ifndef __QAIC_HEADER |
|
||||||
#define __QAIC_HEADER(ff) ff |
|
||||||
#endif //__QAIC_HEADER
|
|
||||||
|
|
||||||
#ifndef __QAIC_HEADER_EXPORT |
|
||||||
#define __QAIC_HEADER_EXPORT |
|
||||||
#endif // __QAIC_HEADER_EXPORT
|
|
||||||
|
|
||||||
#ifndef __QAIC_HEADER_ATTRIBUTE |
|
||||||
#define __QAIC_HEADER_ATTRIBUTE |
|
||||||
#endif // __QAIC_HEADER_ATTRIBUTE
|
|
||||||
|
|
||||||
#ifndef __QAIC_IMPL |
|
||||||
#define __QAIC_IMPL(ff) ff |
|
||||||
#endif //__QAIC_IMPL
|
|
||||||
|
|
||||||
#ifndef __QAIC_IMPL_EXPORT |
|
||||||
#define __QAIC_IMPL_EXPORT |
|
||||||
#endif // __QAIC_IMPL_EXPORT
|
|
||||||
|
|
||||||
#ifndef __QAIC_IMPL_ATTRIBUTE |
|
||||||
#define __QAIC_IMPL_ATTRIBUTE |
|
||||||
#endif // __QAIC_IMPL_ATTRIBUTE
|
|
||||||
|
|
||||||
#ifndef __QAIC_STUB |
|
||||||
#define __QAIC_STUB(ff) ff |
|
||||||
#endif //__QAIC_STUB
|
|
||||||
|
|
||||||
#ifndef __QAIC_STUB_EXPORT |
|
||||||
#define __QAIC_STUB_EXPORT |
|
||||||
#endif // __QAIC_STUB_EXPORT
|
|
||||||
|
|
||||||
#ifndef __QAIC_STUB_ATTRIBUTE |
|
||||||
#define __QAIC_STUB_ATTRIBUTE |
|
||||||
#endif // __QAIC_STUB_ATTRIBUTE
|
|
||||||
|
|
||||||
#ifndef __QAIC_SKEL |
|
||||||
#define __QAIC_SKEL(ff) ff |
|
||||||
#endif //__QAIC_SKEL__
|
|
||||||
|
|
||||||
#ifndef __QAIC_SKEL_EXPORT |
|
||||||
#define __QAIC_SKEL_EXPORT |
|
||||||
#endif // __QAIC_SKEL_EXPORT
|
|
||||||
|
|
||||||
#ifndef __QAIC_SKEL_ATTRIBUTE |
|
||||||
#define __QAIC_SKEL_ATTRIBUTE |
|
||||||
#endif // __QAIC_SKEL_ATTRIBUTE
|
|
||||||
|
|
||||||
#ifdef __QAIC_DEBUG__ |
|
||||||
#ifndef __QAIC_DBG_PRINTF__ |
|
||||||
#include <stdio.h> |
|
||||||
#define __QAIC_DBG_PRINTF__( ee ) do { printf ee ; } while(0) |
|
||||||
#endif |
|
||||||
#else |
|
||||||
#define __QAIC_DBG_PRINTF__( ee ) (void)0 |
|
||||||
#endif |
|
||||||
|
|
||||||
|
|
||||||
#define _OFFSET(src, sof) ((void*)(((char*)(src)) + (sof))) |
|
||||||
|
|
||||||
#define _COPY(dst, dof, src, sof, sz) \ |
|
||||||
do {\
|
|
||||||
struct __copy { \
|
|
||||||
char ar[sz]; \
|
|
||||||
};\
|
|
||||||
*(struct __copy*)_OFFSET(dst, dof) = *(struct __copy*)_OFFSET(src, sof);\
|
|
||||||
} while (0) |
|
||||||
|
|
||||||
#define _COPYIF(dst, dof, src, sof, sz) \ |
|
||||||
do {\
|
|
||||||
if(_OFFSET(dst, dof) != _OFFSET(src, sof)) {\
|
|
||||||
_COPY(dst, dof, src, sof, sz); \
|
|
||||||
} \
|
|
||||||
} while (0) |
|
||||||
|
|
||||||
_ATTRIBUTE_UNUSED |
|
||||||
static __inline void _qaic_memmove(void* dst, void* src, int size) { |
|
||||||
int i; |
|
||||||
for(i = 0; i < size; ++i) { |
|
||||||
((char*)dst)[i] = ((char*)src)[i]; |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
#define _MEMMOVEIF(dst, src, sz) \ |
|
||||||
do {\
|
|
||||||
if(dst != src) {\
|
|
||||||
_qaic_memmove(dst, src, sz);\
|
|
||||||
} \
|
|
||||||
} while (0) |
|
||||||
|
|
||||||
|
|
||||||
#define _ASSIGN(dst, src, sof) \ |
|
||||||
do {\
|
|
||||||
dst = OFFSET(src, sof); \
|
|
||||||
} while (0) |
|
||||||
|
|
||||||
#define _STD_STRLEN_IF(str) (str == 0 ? 0 : strlen(str)) |
|
||||||
|
|
||||||
#define AEE_SUCCESS 0 |
|
||||||
#define AEE_EOFFSET 0x80000400 |
|
||||||
#define AEE_EBADPARM (AEE_EOFFSET + 0x00E) |
|
||||||
|
|
||||||
#define _TRY(ee, func) \ |
|
||||||
do { \
|
|
||||||
if (AEE_SUCCESS != ((ee) = func)) {\
|
|
||||||
__QAIC_DBG_PRINTF__((__FILE__ ":%d:error:%d:%s\n", __LINE__, (int)(ee),#func));\
|
|
||||||
goto ee##bail;\
|
|
||||||
} \
|
|
||||||
} while (0) |
|
||||||
|
|
||||||
#define _CATCH(exception) exception##bail: if (exception != AEE_SUCCESS) |
|
||||||
|
|
||||||
#define _ASSERT(nErr, ff) _TRY(nErr, 0 == (ff) ? AEE_EBADPARM : AEE_SUCCESS) |
|
||||||
|
|
||||||
#ifdef __QAIC_DEBUG__ |
|
||||||
#define _ALLOCATE(nErr, pal, size, alignment, pv) _TRY(nErr, _allocator_alloc(pal, __FILE_LINE__, size, alignment, (void**)&pv)) |
|
||||||
#else |
|
||||||
#define _ALLOCATE(nErr, pal, size, alignment, pv) _TRY(nErr, _allocator_alloc(pal, 0, size, alignment, (void**)&pv)) |
|
||||||
#endif |
|
||||||
|
|
||||||
|
|
||||||
#endif // _QAIC_ENV_H
|
|
||||||
|
|
||||||
#ifndef _ALLOCATOR_H |
|
||||||
#define _ALLOCATOR_H |
|
||||||
|
|
||||||
#include <stdlib.h> |
|
||||||
#include <stdint.h> |
|
||||||
|
|
||||||
typedef struct _heap _heap; |
|
||||||
struct _heap { |
|
||||||
_heap* pPrev; |
|
||||||
const char* loc; |
|
||||||
uint64_t buf; |
|
||||||
}; |
|
||||||
|
|
||||||
typedef struct _allocator { |
|
||||||
_heap* pheap; |
|
||||||
uint8_t* stack; |
|
||||||
uint8_t* stackEnd; |
|
||||||
int nSize; |
|
||||||
} _allocator; |
|
||||||
|
|
||||||
_ATTRIBUTE_UNUSED |
|
||||||
static __inline int _heap_alloc(_heap** ppa, const char* loc, int size, void** ppbuf) { |
|
||||||
_heap* pn = 0; |
|
||||||
pn = malloc(size + sizeof(_heap) - sizeof(uint64_t)); |
|
||||||
if(pn != 0) { |
|
||||||
pn->pPrev = *ppa; |
|
||||||
pn->loc = loc; |
|
||||||
*ppa = pn; |
|
||||||
*ppbuf = (void*)&(pn->buf); |
|
||||||
return 0; |
|
||||||
} else { |
|
||||||
return -1; |
|
||||||
} |
|
||||||
} |
|
||||||
#define _ALIGN_SIZE(x, y) (((x) + (y-1)) & ~(y-1)) |
|
||||||
|
|
||||||
_ATTRIBUTE_UNUSED |
|
||||||
static __inline int _allocator_alloc(_allocator* me, |
|
||||||
const char* loc, |
|
||||||
int size, |
|
||||||
unsigned int al, |
|
||||||
void** ppbuf) { |
|
||||||
if(size < 0) { |
|
||||||
return -1; |
|
||||||
} else if (size == 0) { |
|
||||||
*ppbuf = 0; |
|
||||||
return 0; |
|
||||||
} |
|
||||||
if((_ALIGN_SIZE((uintptr_t)me->stackEnd, al) + size) < (uintptr_t)me->stack + me->nSize) { |
|
||||||
*ppbuf = (uint8_t*)_ALIGN_SIZE((uintptr_t)me->stackEnd, al); |
|
||||||
me->stackEnd = (uint8_t*)_ALIGN_SIZE((uintptr_t)me->stackEnd, al) + size; |
|
||||||
return 0; |
|
||||||
} else { |
|
||||||
return _heap_alloc(&me->pheap, loc, size, ppbuf); |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
_ATTRIBUTE_UNUSED |
|
||||||
static __inline void _allocator_deinit(_allocator* me) { |
|
||||||
_heap* pa = me->pheap; |
|
||||||
while(pa != 0) { |
|
||||||
_heap* pn = pa; |
|
||||||
const char* loc = pn->loc; |
|
||||||
(void)loc; |
|
||||||
pa = pn->pPrev; |
|
||||||
free(pn); |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
_ATTRIBUTE_UNUSED |
|
||||||
static __inline void _allocator_init(_allocator* me, uint8_t* stack, int stackSize) { |
|
||||||
me->stack = stack; |
|
||||||
me->stackEnd = stack + stackSize; |
|
||||||
me->nSize = stackSize; |
|
||||||
me->pheap = 0; |
|
||||||
} |
|
||||||
|
|
||||||
|
|
||||||
#endif // _ALLOCATOR_H
|
|
||||||
|
|
||||||
#ifndef SLIM_H |
|
||||||
#define SLIM_H |
|
||||||
|
|
||||||
#include <stdint.h> |
|
||||||
|
|
||||||
//a C data structure for the idl types that can be used to implement
|
|
||||||
//static and dynamic language bindings fairly efficiently.
|
|
||||||
//
|
|
||||||
//the goal is to have a minimal ROM and RAM footprint and without
|
|
||||||
//doing too many allocations. A good way to package these things seemed
|
|
||||||
//like the module boundary, so all the idls within one module can share
|
|
||||||
//all the type references.
|
|
||||||
|
|
||||||
|
|
||||||
#define PARAMETER_IN 0x0 |
|
||||||
#define PARAMETER_OUT 0x1 |
|
||||||
#define PARAMETER_INOUT 0x2 |
|
||||||
#define PARAMETER_ROUT 0x3 |
|
||||||
#define PARAMETER_INROUT 0x4 |
|
||||||
|
|
||||||
//the types that we get from idl
|
|
||||||
#define TYPE_OBJECT 0x0 |
|
||||||
#define TYPE_INTERFACE 0x1 |
|
||||||
#define TYPE_PRIMITIVE 0x2 |
|
||||||
#define TYPE_ENUM 0x3 |
|
||||||
#define TYPE_STRING 0x4 |
|
||||||
#define TYPE_WSTRING 0x5 |
|
||||||
#define TYPE_STRUCTURE 0x6 |
|
||||||
#define TYPE_UNION 0x7 |
|
||||||
#define TYPE_ARRAY 0x8 |
|
||||||
#define TYPE_SEQUENCE 0x9 |
|
||||||
|
|
||||||
//these require the pack/unpack to recurse
|
|
||||||
//so it's a hint to those languages that can optimize in cases where
|
|
||||||
//recursion isn't necessary.
|
|
||||||
#define TYPE_COMPLEX_STRUCTURE (0x10 | TYPE_STRUCTURE) |
|
||||||
#define TYPE_COMPLEX_UNION (0x10 | TYPE_UNION) |
|
||||||
#define TYPE_COMPLEX_ARRAY (0x10 | TYPE_ARRAY) |
|
||||||
#define TYPE_COMPLEX_SEQUENCE (0x10 | TYPE_SEQUENCE) |
|
||||||
|
|
||||||
|
|
||||||
typedef struct Type Type; |
|
||||||
|
|
||||||
#define INHERIT_TYPE\ |
|
||||||
int32_t nativeSize; /*in the simple case its the same as wire size and alignment*/\
|
|
||||||
union {\
|
|
||||||
struct {\
|
|
||||||
const uintptr_t p1;\
|
|
||||||
const uintptr_t p2;\
|
|
||||||
} _cast;\
|
|
||||||
struct {\
|
|
||||||
uint32_t iid;\
|
|
||||||
uint32_t bNotNil;\
|
|
||||||
} object;\
|
|
||||||
struct {\
|
|
||||||
const Type *arrayType;\
|
|
||||||
int32_t nItems;\
|
|
||||||
} array;\
|
|
||||||
struct {\
|
|
||||||
const Type *seqType;\
|
|
||||||
int32_t nMaxLen;\
|
|
||||||
} seqSimple; \
|
|
||||||
struct {\
|
|
||||||
uint32_t bFloating;\
|
|
||||||
uint32_t bSigned;\
|
|
||||||
} prim; \
|
|
||||||
const SequenceType* seqComplex;\
|
|
||||||
const UnionType *unionType;\
|
|
||||||
const StructType *structType;\
|
|
||||||
int32_t stringMaxLen;\
|
|
||||||
uint8_t bInterfaceNotNil;\
|
|
||||||
} param;\
|
|
||||||
uint8_t type;\
|
|
||||||
uint8_t nativeAlignment\
|
|
||||||
|
|
||||||
typedef struct UnionType UnionType; |
|
||||||
typedef struct StructType StructType; |
|
||||||
typedef struct SequenceType SequenceType; |
|
||||||
struct Type { |
|
||||||
INHERIT_TYPE; |
|
||||||
}; |
|
||||||
|
|
||||||
struct SequenceType { |
|
||||||
const Type * seqType; |
|
||||||
uint32_t nMaxLen; |
|
||||||
uint32_t inSize; |
|
||||||
uint32_t routSizePrimIn; |
|
||||||
uint32_t routSizePrimROut; |
|
||||||
}; |
|
||||||
|
|
||||||
//byte offset from the start of the case values for
|
|
||||||
//this unions case value array. it MUST be aligned
|
|
||||||
//at the alignment requrements for the descriptor
|
|
||||||
//
|
|
||||||
//if negative it means that the unions cases are
|
|
||||||
//simple enumerators, so the value read from the descriptor
|
|
||||||
//can be used directly to find the correct case
|
|
||||||
typedef union CaseValuePtr CaseValuePtr; |
|
||||||
union CaseValuePtr { |
|
||||||
const uint8_t* value8s; |
|
||||||
const uint16_t* value16s; |
|
||||||
const uint32_t* value32s; |
|
||||||
const uint64_t* value64s; |
|
||||||
}; |
|
||||||
|
|
||||||
//these are only used in complex cases
|
|
||||||
//so I pulled them out of the type definition as references to make
|
|
||||||
//the type smaller
|
|
||||||
struct UnionType { |
|
||||||
const Type *descriptor; |
|
||||||
uint32_t nCases; |
|
||||||
const CaseValuePtr caseValues; |
|
||||||
const Type * const *cases; |
|
||||||
int32_t inSize; |
|
||||||
int32_t routSizePrimIn; |
|
||||||
int32_t routSizePrimROut; |
|
||||||
uint8_t inAlignment; |
|
||||||
uint8_t routAlignmentPrimIn; |
|
||||||
uint8_t routAlignmentPrimROut; |
|
||||||
uint8_t inCaseAlignment; |
|
||||||
uint8_t routCaseAlignmentPrimIn; |
|
||||||
uint8_t routCaseAlignmentPrimROut; |
|
||||||
uint8_t nativeCaseAlignment; |
|
||||||
uint8_t bDefaultCase; |
|
||||||
}; |
|
||||||
|
|
||||||
struct StructType { |
|
||||||
uint32_t nMembers; |
|
||||||
const Type * const *members; |
|
||||||
int32_t inSize; |
|
||||||
int32_t routSizePrimIn; |
|
||||||
int32_t routSizePrimROut; |
|
||||||
uint8_t inAlignment; |
|
||||||
uint8_t routAlignmentPrimIn; |
|
||||||
uint8_t routAlignmentPrimROut; |
|
||||||
}; |
|
||||||
|
|
||||||
typedef struct Parameter Parameter; |
|
||||||
struct Parameter { |
|
||||||
INHERIT_TYPE; |
|
||||||
uint8_t mode; |
|
||||||
uint8_t bNotNil; |
|
||||||
}; |
|
||||||
|
|
||||||
#define SLIM_IFPTR32(is32,is64) (sizeof(uintptr_t) == 4 ? (is32) : (is64)) |
|
||||||
#define SLIM_SCALARS_IS_DYNAMIC(u) (((u) & 0x00ffffff) == 0x00ffffff) |
|
||||||
|
|
||||||
typedef struct Method Method; |
|
||||||
struct Method { |
|
||||||
uint32_t uScalars; //no method index
|
|
||||||
int32_t primInSize; |
|
||||||
int32_t primROutSize; |
|
||||||
int maxArgs; |
|
||||||
int numParams; |
|
||||||
const Parameter * const *params; |
|
||||||
uint8_t primInAlignment; |
|
||||||
uint8_t primROutAlignment; |
|
||||||
}; |
|
||||||
|
|
||||||
typedef struct Interface Interface; |
|
||||||
|
|
||||||
struct Interface { |
|
||||||
int nMethods; |
|
||||||
const Method * const *methodArray; |
|
||||||
int nIIds; |
|
||||||
const uint32_t *iids; |
|
||||||
const uint16_t* methodStringArray; |
|
||||||
const uint16_t* methodStrings; |
|
||||||
const char* strings; |
|
||||||
}; |
|
||||||
|
|
||||||
|
|
||||||
#endif //SLIM_H
|
|
||||||
|
|
||||||
|
|
||||||
#ifndef _CALCULATOR_SLIM_H |
|
||||||
#define _CALCULATOR_SLIM_H |
|
||||||
|
|
||||||
// remote.h
|
|
||||||
|
|
||||||
#include <stdint.h> |
|
||||||
|
|
||||||
#ifndef __QAIC_SLIM |
|
||||||
#define __QAIC_SLIM(ff) ff |
|
||||||
#endif |
|
||||||
#ifndef __QAIC_SLIM_EXPORT |
|
||||||
#define __QAIC_SLIM_EXPORT |
|
||||||
#endif |
|
||||||
|
|
||||||
static const Type types[1]; |
|
||||||
static const Type types[1] = {{0x1,{{(const uintptr_t)0,(const uintptr_t)0}}, 2,0x1}}; |
|
||||||
static const Parameter parameters[3] = {{0x4,{{(const uintptr_t)0,(const uintptr_t)0}}, 2,0x4,3,0},{SLIM_IFPTR32(0x8,0x10),{{(const uintptr_t)&(types[0]),(const uintptr_t)0x0}}, 9,SLIM_IFPTR32(0x4,0x8),0,0},{SLIM_IFPTR32(0x8,0x10),{{(const uintptr_t)&(types[0]),(const uintptr_t)0x0}}, 9,SLIM_IFPTR32(0x4,0x8),3,0}}; |
|
||||||
static const Parameter* const parameterArrays[3] = {(&(parameters[1])),(&(parameters[2])),(&(parameters[0]))}; |
|
||||||
static const Method methods[2] = {{REMOTE_SCALARS_MAKEX(0,0,0x0,0x1,0x0,0x0),0x0,0x4,1,1,(&(parameterArrays[2])),0x1,0x4},{REMOTE_SCALARS_MAKEX(0,0,0x2,0x1,0x0,0x0),0x8,0x0,5,2,(&(parameterArrays[0])),0x4,0x1}}; |
|
||||||
static const Method* const methodArrays[2] = {&(methods[0]),&(methods[1])}; |
|
||||||
static const char strings[41] = "extract_and_match\0features\0leet\0init\0img\0"; |
|
||||||
static const uint16_t methodStrings[5] = {0,37,18,32,27}; |
|
||||||
static const uint16_t methodStringsArrays[2] = {3,0}; |
|
||||||
__QAIC_SLIM_EXPORT const Interface __QAIC_SLIM(calculator_slim) = {2,&(methodArrays[0]),0,0,&(methodStringsArrays [0]),methodStrings,strings}; |
|
||||||
#endif //_CALCULATOR_SLIM_H
|
|
||||||
#ifdef __cplusplus |
|
||||||
extern "C" { |
|
||||||
#endif |
|
||||||
|
|
||||||
#ifndef _const_calculator_handle |
|
||||||
#define _const_calculator_handle ((remote_handle)-1) |
|
||||||
#endif //_const_calculator_handle
|
|
||||||
|
|
||||||
static void _calculator_pls_dtor(void* data) { |
|
||||||
remote_handle* ph = (remote_handle*)data; |
|
||||||
if(_const_calculator_handle != *ph) { |
|
||||||
(void)__QAIC_REMOTE(remote_handle_close)(*ph); |
|
||||||
*ph = _const_calculator_handle; |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
static int _calculator_pls_ctor(void* ctx, void* data) { |
|
||||||
remote_handle* ph = (remote_handle*)data; |
|
||||||
*ph = _const_calculator_handle; |
|
||||||
if(*ph == (remote_handle)-1) { |
|
||||||
return __QAIC_REMOTE(remote_handle_open)((const char*)ctx, ph); |
|
||||||
} |
|
||||||
return 0; |
|
||||||
} |
|
||||||
|
|
||||||
#if (defined __qdsp6__) || (defined __hexagon__) |
|
||||||
#pragma weak adsp_pls_add_lookup |
|
||||||
extern int adsp_pls_add_lookup(uint32_t type, uint32_t key, int size, int (*ctor)(void* ctx, void* data), void* ctx, void (*dtor)(void* ctx), void** ppo); |
|
||||||
#pragma weak HAP_pls_add_lookup |
|
||||||
extern int HAP_pls_add_lookup(uint32_t type, uint32_t key, int size, int (*ctor)(void* ctx, void* data), void* ctx, void (*dtor)(void* ctx), void** ppo); |
|
||||||
|
|
||||||
__QAIC_STUB_EXPORT remote_handle _calculator_handle(void) { |
|
||||||
remote_handle* ph; |
|
||||||
if(adsp_pls_add_lookup) { |
|
||||||
if(0 == adsp_pls_add_lookup((uint32_t)_calculator_handle, 0, sizeof(*ph), _calculator_pls_ctor, "calculator", _calculator_pls_dtor, (void**)&ph)) { |
|
||||||
return *ph; |
|
||||||
} |
|
||||||
return (remote_handle)-1; |
|
||||||
} else if(HAP_pls_add_lookup) { |
|
||||||
if(0 == HAP_pls_add_lookup((uint32_t)_calculator_handle, 0, sizeof(*ph), _calculator_pls_ctor, "calculator", _calculator_pls_dtor, (void**)&ph)) { |
|
||||||
return *ph; |
|
||||||
} |
|
||||||
return (remote_handle)-1; |
|
||||||
} |
|
||||||
return(remote_handle)-1; |
|
||||||
} |
|
||||||
|
|
||||||
#else //__qdsp6__ || __hexagon__
|
|
||||||
|
|
||||||
uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare); |
|
||||||
|
|
||||||
#ifdef _WIN32 |
|
||||||
#include "Windows.h" |
|
||||||
uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare) { |
|
||||||
return (uint32_t)InterlockedCompareExchange((volatile LONG*)puDest, (LONG)uExchange, (LONG)uCompare); |
|
||||||
} |
|
||||||
#elif __GNUC__ |
|
||||||
uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare) { |
|
||||||
return __sync_val_compare_and_swap(puDest, uCompare, uExchange); |
|
||||||
} |
|
||||||
#endif //_WIN32
|
|
||||||
|
|
||||||
|
|
||||||
__QAIC_STUB_EXPORT remote_handle _calculator_handle(void) { |
|
||||||
static remote_handle handle = _const_calculator_handle; |
|
||||||
if((remote_handle)-1 != handle) { |
|
||||||
return handle; |
|
||||||
} else { |
|
||||||
remote_handle tmp; |
|
||||||
int nErr = _calculator_pls_ctor("calculator", (void*)&tmp); |
|
||||||
if(nErr) { |
|
||||||
return (remote_handle)-1; |
|
||||||
} |
|
||||||
if(((remote_handle)-1 != handle) || ((remote_handle)-1 != (remote_handle)_calculator_atomic_CompareAndExchange((uint32_t*)&handle, (uint32_t)tmp, (uint32_t)-1))) { |
|
||||||
_calculator_pls_dtor(&tmp); |
|
||||||
} |
|
||||||
return handle; |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
#endif //__qdsp6__
|
|
||||||
|
|
||||||
__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_skel_invoke)(uint32_t _sc, remote_arg* _pra) __QAIC_STUB_ATTRIBUTE { |
|
||||||
return __QAIC_REMOTE(remote_handle_invoke)(_calculator_handle(), _sc, _pra); |
|
||||||
} |
|
||||||
|
|
||||||
#ifdef __cplusplus |
|
||||||
} |
|
||||||
#endif |
|
||||||
|
|
||||||
|
|
||||||
#ifdef __cplusplus |
|
||||||
extern "C" { |
|
||||||
#endif |
|
||||||
extern int remote_register_dma_handle(int, uint32_t); |
|
||||||
static __inline int _stub_method(remote_handle _handle, uint32_t _mid, uint32_t _rout0[1]) { |
|
||||||
int _numIn[1]; |
|
||||||
remote_arg _pra[1]; |
|
||||||
uint32_t _primROut[1]; |
|
||||||
int _nErr = 0; |
|
||||||
_numIn[0] = 0; |
|
||||||
_pra[(_numIn[0] + 0)].buf.pv = (void*)_primROut; |
|
||||||
_pra[(_numIn[0] + 0)].buf.nLen = sizeof(_primROut); |
|
||||||
_TRY(_nErr, __QAIC_REMOTE(remote_handle_invoke)(_handle, REMOTE_SCALARS_MAKEX(0, _mid, 0, 1, 0, 0), _pra)); |
|
||||||
_COPY(_rout0, 0, _primROut, 0, 4); |
|
||||||
_CATCH(_nErr) {} |
|
||||||
return _nErr; |
|
||||||
} |
|
||||||
__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_init)(uint32* leet) __QAIC_STUB_ATTRIBUTE { |
|
||||||
uint32_t _mid = 0; |
|
||||||
return _stub_method(_calculator_handle(), _mid, (uint32_t*)leet); |
|
||||||
} |
|
||||||
static __inline int _stub_method_1(remote_handle _handle, uint32_t _mid, char* _in0[1], uint32_t _in0Len[1], char* _rout1[1], uint32_t _rout1Len[1]) { |
|
||||||
int _numIn[1]; |
|
||||||
remote_arg _pra[3]; |
|
||||||
uint32_t _primIn[2]; |
|
||||||
remote_arg* _praIn; |
|
||||||
remote_arg* _praROut; |
|
||||||
int _nErr = 0; |
|
||||||
_numIn[0] = 1; |
|
||||||
_pra[0].buf.pv = (void*)_primIn; |
|
||||||
_pra[0].buf.nLen = sizeof(_primIn); |
|
||||||
_COPY(_primIn, 0, _in0Len, 0, 4); |
|
||||||
_praIn = (_pra + 1); |
|
||||||
_praIn[0].buf.pv = _in0[0]; |
|
||||||
_praIn[0].buf.nLen = (1 * _in0Len[0]); |
|
||||||
_COPY(_primIn, 4, _rout1Len, 0, 4); |
|
||||||
_praROut = (_praIn + _numIn[0] + 0); |
|
||||||
_praROut[0].buf.pv = _rout1[0]; |
|
||||||
_praROut[0].buf.nLen = (1 * _rout1Len[0]); |
|
||||||
_TRY(_nErr, __QAIC_REMOTE(remote_handle_invoke)(_handle, REMOTE_SCALARS_MAKEX(0, _mid, 2, 1, 0, 0), _pra)); |
|
||||||
_CATCH(_nErr) {} |
|
||||||
return _nErr; |
|
||||||
} |
|
||||||
__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_extract_and_match)(const uint8* img, int imgLen, uint8* features, int featuresLen) __QAIC_STUB_ATTRIBUTE { |
|
||||||
uint32_t _mid = 1; |
|
||||||
return _stub_method_1(_calculator_handle(), _mid, (char**)&img, (uint32_t*)&imgLen, (char**)&features, (uint32_t*)&featuresLen); |
|
||||||
} |
|
||||||
#ifdef __cplusplus |
|
||||||
} |
|
||||||
#endif |
|
||||||
#endif //_CALCULATOR_STUB_H
|
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:b94bcedb39d110b074a99fe3129da2506bc4b06cca7f84731cd0620683fd1179 |
|
||||||
size 357400 |
|
||||||
@ -1,37 +0,0 @@ |
|||||||
#ifndef EXTRACTOR_H |
|
||||||
#define EXTRACTOR_H |
|
||||||
|
|
||||||
#ifdef __cplusplus |
|
||||||
extern "C" { |
|
||||||
#endif |
|
||||||
|
|
||||||
#include <stdint.h> |
|
||||||
|
|
||||||
#define ORBD_KEYPOINTS 3000 |
|
||||||
#define ORBD_DESCRIPTOR_LENGTH 32 |
|
||||||
#define ORBD_HEIGHT 874 |
|
||||||
#define ORBD_WIDTH 1164 |
|
||||||
|
|
||||||
// matches OrbFeatures from log.capnp
|
|
||||||
struct orb_features { |
|
||||||
// align this
|
|
||||||
uint16_t n_corners; |
|
||||||
uint16_t xy[ORBD_KEYPOINTS][2]; |
|
||||||
uint8_t octave[ORBD_KEYPOINTS]; |
|
||||||
uint8_t des[ORBD_KEYPOINTS][ORBD_DESCRIPTOR_LENGTH]; |
|
||||||
int16_t matches[ORBD_KEYPOINTS]; |
|
||||||
}; |
|
||||||
|
|
||||||
// forward declare this
|
|
||||||
struct pyramid; |
|
||||||
|
|
||||||
// manage the pyramids in extractor.c
|
|
||||||
void init_gpyrs(); |
|
||||||
int extract_and_match_gpyrs(const uint8_t *img, struct orb_features *); |
|
||||||
int extract_and_match(const uint8_t *img, struct pyramid *pyrs, struct pyramid *prev_pyrs, struct orb_features *); |
|
||||||
|
|
||||||
#ifdef __cplusplus |
|
||||||
} |
|
||||||
#endif |
|
||||||
|
|
||||||
#endif // EXTRACTOR_H
|
|
||||||
@ -1,181 +0,0 @@ |
|||||||
#include <stdio.h> |
|
||||||
#include <stdlib.h> |
|
||||||
#include <signal.h> |
|
||||||
#include <unistd.h> |
|
||||||
#include <stdint.h> |
|
||||||
#include <assert.h> |
|
||||||
#include <sys/resource.h> |
|
||||||
|
|
||||||
#include "common/visionipc.h" |
|
||||||
#include "common/swaglog.h" |
|
||||||
|
|
||||||
#include "extractor.h" |
|
||||||
|
|
||||||
#ifdef DSP |
|
||||||
#include "dsp/gen/calculator.h" |
|
||||||
#else |
|
||||||
#include "turbocv.h" |
|
||||||
#endif |
|
||||||
|
|
||||||
#include <zmq.h> |
|
||||||
#include <capnp/serialize.h> |
|
||||||
#include "cereal/gen/cpp/log.capnp.h" |
|
||||||
|
|
||||||
#ifndef PATH_MAX |
|
||||||
#include <linux/limits.h> |
|
||||||
#endif |
|
||||||
|
|
||||||
volatile int do_exit = 0; |
|
||||||
|
|
||||||
static void set_do_exit(int sig) { |
|
||||||
do_exit = 1; |
|
||||||
} |
|
||||||
|
|
||||||
int main(int argc, char *argv[]) { |
|
||||||
int err; |
|
||||||
setpriority(PRIO_PROCESS, 0, -13); |
|
||||||
printf("starting orbd\n"); |
|
||||||
|
|
||||||
#ifdef DSP |
|
||||||
uint32_t test_leet = 0; |
|
||||||
char my_path[PATH_MAX+1]; |
|
||||||
memset(my_path, 0, sizeof(my_path)); |
|
||||||
|
|
||||||
ssize_t len = readlink("/proc/self/exe", my_path, sizeof(my_path)); |
|
||||||
assert(len > 5); |
|
||||||
my_path[len-5] = '\0'; |
|
||||||
LOGW("running from %s with PATH_MAX %d", my_path, PATH_MAX); |
|
||||||
|
|
||||||
char adsp_path[PATH_MAX+1]; |
|
||||||
snprintf(adsp_path, PATH_MAX, "ADSP_LIBRARY_PATH=%s/dsp/gen", my_path); |
|
||||||
assert(putenv(adsp_path) == 0); |
|
||||||
|
|
||||||
assert(calculator_init(&test_leet) == 0); |
|
||||||
assert(test_leet == 0x1337); |
|
||||||
LOGW("orbd init complete"); |
|
||||||
#else |
|
||||||
init_gpyrs(); |
|
||||||
#endif |
|
||||||
|
|
||||||
signal(SIGINT, (sighandler_t) set_do_exit); |
|
||||||
signal(SIGTERM, (sighandler_t) set_do_exit); |
|
||||||
|
|
||||||
void *ctx = zmq_ctx_new(); |
|
||||||
|
|
||||||
void *orb_features_sock = zmq_socket(ctx, ZMQ_PUB); |
|
||||||
assert(orb_features_sock); |
|
||||||
zmq_bind(orb_features_sock, "tcp://*:8058"); |
|
||||||
|
|
||||||
void *orb_features_summary_sock = zmq_socket(ctx, ZMQ_PUB); |
|
||||||
assert(orb_features_summary_sock); |
|
||||||
zmq_bind(orb_features_summary_sock, "tcp://*:8062"); |
|
||||||
|
|
||||||
struct orb_features *features = (struct orb_features *)malloc(sizeof(struct orb_features)); |
|
||||||
int last_frame_id = 0; |
|
||||||
|
|
||||||
VisionStream stream; |
|
||||||
while (!do_exit) { |
|
||||||
VisionStreamBufs buf_info; |
|
||||||
err = visionstream_init(&stream, VISION_STREAM_YUV, true, &buf_info); |
|
||||||
if (err) { |
|
||||||
printf("visionstream connect fail\n"); |
|
||||||
usleep(100000); |
|
||||||
continue; |
|
||||||
} |
|
||||||
uint64_t timestamp_last_eof = 0; |
|
||||||
while (!do_exit) { |
|
||||||
VIPCBuf *buf; |
|
||||||
VIPCBufExtra extra; |
|
||||||
buf = visionstream_get(&stream, &extra); |
|
||||||
if (buf == NULL) { |
|
||||||
printf("visionstream get failed\n"); |
|
||||||
break; |
|
||||||
} |
|
||||||
|
|
||||||
uint64_t start = nanos_since_boot(); |
|
||||||
#ifdef DSP |
|
||||||
int ret = calculator_extract_and_match((uint8_t *)buf->addr, ORBD_HEIGHT*ORBD_WIDTH, (uint8_t *)features, sizeof(struct orb_features)); |
|
||||||
#else |
|
||||||
int ret = extract_and_match_gpyrs((uint8_t *) buf->addr, features); |
|
||||||
#endif |
|
||||||
uint64_t end = nanos_since_boot(); |
|
||||||
LOGD("total(%d): %6.2f ms to get %4d features on %d", ret, (end-start)/1000000.0, features->n_corners, extra.frame_id); |
|
||||||
assert(ret == 0); |
|
||||||
|
|
||||||
if (last_frame_id+1 != extra.frame_id) { |
|
||||||
LOGW("dropped frame!"); |
|
||||||
} |
|
||||||
|
|
||||||
last_frame_id = extra.frame_id; |
|
||||||
|
|
||||||
if (timestamp_last_eof == 0) { |
|
||||||
timestamp_last_eof = extra.timestamp_eof; |
|
||||||
continue; |
|
||||||
} |
|
||||||
|
|
||||||
int match_count = 0; |
|
||||||
|
|
||||||
// *** send OrbFeatures ***
|
|
||||||
{ |
|
||||||
// create capnp message
|
|
||||||
capnp::MallocMessageBuilder msg; |
|
||||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>(); |
|
||||||
event.setLogMonoTime(nanos_since_boot()); |
|
||||||
|
|
||||||
auto orb_features = event.initOrbFeatures(); |
|
||||||
|
|
||||||
// set timestamps
|
|
||||||
orb_features.setTimestampEof(extra.timestamp_eof); |
|
||||||
orb_features.setTimestampLastEof(timestamp_last_eof); |
|
||||||
|
|
||||||
// init descriptors for send
|
|
||||||
kj::ArrayPtr<capnp::byte> descriptorsPtr = kj::arrayPtr((uint8_t *)features->des, ORBD_DESCRIPTOR_LENGTH * features->n_corners); |
|
||||||
orb_features.setDescriptors(descriptorsPtr); |
|
||||||
|
|
||||||
auto xs = orb_features.initXs(features->n_corners); |
|
||||||
auto ys = orb_features.initYs(features->n_corners); |
|
||||||
auto octaves = orb_features.initOctaves(features->n_corners); |
|
||||||
auto matches = orb_features.initMatches(features->n_corners); |
|
||||||
|
|
||||||
// copy out normalized keypoints
|
|
||||||
for (int i = 0; i < features->n_corners; i++) { |
|
||||||
xs.set(i, features->xy[i][0] * 1.0f / ORBD_WIDTH - 0.5f); |
|
||||||
ys.set(i, features->xy[i][1] * 1.0f / ORBD_HEIGHT - 0.5f); |
|
||||||
octaves.set(i, features->octave[i]); |
|
||||||
matches.set(i, features->matches[i]); |
|
||||||
match_count += features->matches[i] != -1; |
|
||||||
} |
|
||||||
|
|
||||||
auto words = capnp::messageToFlatArray(msg); |
|
||||||
auto bytes = words.asBytes(); |
|
||||||
zmq_send(orb_features_sock, bytes.begin(), bytes.size(), 0); |
|
||||||
} |
|
||||||
|
|
||||||
// *** send OrbFeaturesSummary ***
|
|
||||||
|
|
||||||
{ |
|
||||||
// create capnp message
|
|
||||||
capnp::MallocMessageBuilder msg; |
|
||||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>(); |
|
||||||
event.setLogMonoTime(nanos_since_boot()); |
|
||||||
|
|
||||||
auto orb_features_summary = event.initOrbFeaturesSummary(); |
|
||||||
|
|
||||||
orb_features_summary.setTimestampEof(extra.timestamp_eof); |
|
||||||
orb_features_summary.setTimestampLastEof(timestamp_last_eof); |
|
||||||
orb_features_summary.setFeatureCount(features->n_corners); |
|
||||||
orb_features_summary.setMatchCount(match_count); |
|
||||||
orb_features_summary.setComputeNs(end-start); |
|
||||||
|
|
||||||
auto words = capnp::messageToFlatArray(msg); |
|
||||||
auto bytes = words.asBytes(); |
|
||||||
zmq_send(orb_features_summary_sock, bytes.begin(), bytes.size(), 0); |
|
||||||
} |
|
||||||
|
|
||||||
timestamp_last_eof = extra.timestamp_eof; |
|
||||||
} |
|
||||||
} |
|
||||||
visionstream_destroy(&stream); |
|
||||||
return 0; |
|
||||||
} |
|
||||||
|
|
||||||
@ -1,3 +1,3 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
version https://git-lfs.github.com/spec/v1 |
||||||
oid sha256:d51ec7f5e069ab8d9b03b672cb65df0ddae9abe82b220debb9aecc1a36f20f45 |
oid sha256:a539d59efc3c5009af06ea1dffb2acecbdc3798aaf414d08d0c9cb258e7771cb |
||||||
size 1171544 |
size 1171544 |
||||||
|
|||||||
@ -1,3 +1,3 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
version https://git-lfs.github.com/spec/v1 |
||||||
oid sha256:05853b1b3324b420d1463daf806797bb3d85659a0637ca7836b137a84e619be0 |
oid sha256:aafc76b775889ecfb96feb3a2072c3b168a0e026cf2c0ad2f603349cdf6ab779 |
||||||
size 1159016 |
size 1159016 |
||||||
|
|||||||
@ -1,19 +0,0 @@ |
|||||||
"""Methods for reading system thermal information.""" |
|
||||||
import selfdrive.messaging as messaging |
|
||||||
|
|
||||||
def read_tz(x): |
|
||||||
with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f: |
|
||||||
ret = max(0, int(f.read())) |
|
||||||
return ret |
|
||||||
|
|
||||||
def read_thermal(): |
|
||||||
dat = messaging.new_message() |
|
||||||
dat.init('thermal') |
|
||||||
dat.thermal.cpu0 = read_tz(5) |
|
||||||
dat.thermal.cpu1 = read_tz(7) |
|
||||||
dat.thermal.cpu2 = read_tz(10) |
|
||||||
dat.thermal.cpu3 = read_tz(12) |
|
||||||
dat.thermal.mem = read_tz(2) |
|
||||||
dat.thermal.gpu = read_tz(16) |
|
||||||
dat.thermal.bat = read_tz(29) |
|
||||||
return dat |
|
||||||
@ -0,0 +1,272 @@ |
|||||||
|
#!/usr/bin/env python2.7 |
||||||
|
import os |
||||||
|
import zmq |
||||||
|
from smbus2 import SMBus |
||||||
|
|
||||||
|
from selfdrive.version import training_version |
||||||
|
from selfdrive.swaglog import cloudlog |
||||||
|
import selfdrive.messaging as messaging |
||||||
|
from selfdrive.services import service_list |
||||||
|
from selfdrive.loggerd.config import ROOT |
||||||
|
from common.params import Params |
||||||
|
from common.realtime import sec_since_boot |
||||||
|
|
||||||
|
import cereal |
||||||
|
ThermalStatus = cereal.log.ThermalData.ThermalStatus |
||||||
|
|
||||||
|
def read_tz(x): |
||||||
|
with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f: |
||||||
|
ret = max(0, int(f.read())) |
||||||
|
return ret |
||||||
|
|
||||||
|
def read_thermal(): |
||||||
|
dat = messaging.new_message() |
||||||
|
dat.init('thermal') |
||||||
|
dat.thermal.cpu0 = read_tz(5) |
||||||
|
dat.thermal.cpu1 = read_tz(7) |
||||||
|
dat.thermal.cpu2 = read_tz(10) |
||||||
|
dat.thermal.cpu3 = read_tz(12) |
||||||
|
dat.thermal.mem = read_tz(2) |
||||||
|
dat.thermal.gpu = read_tz(16) |
||||||
|
dat.thermal.bat = read_tz(29) |
||||||
|
return dat |
||||||
|
|
||||||
|
LEON = False |
||||||
|
def setup_eon_fan(): |
||||||
|
global LEON |
||||||
|
|
||||||
|
os.system("echo 2 > /sys/module/dwc3_msm/parameters/otg_switch") |
||||||
|
|
||||||
|
bus = SMBus(7, force=True) |
||||||
|
try: |
||||||
|
bus.write_byte_data(0x21, 0x10, 0xf) # mask all interrupts |
||||||
|
bus.write_byte_data(0x21, 0x03, 0x1) # set drive current and global interrupt disable |
||||||
|
bus.write_byte_data(0x21, 0x02, 0x2) # needed? |
||||||
|
bus.write_byte_data(0x21, 0x04, 0x4) # manual override source |
||||||
|
except IOError: |
||||||
|
print "LEON detected" |
||||||
|
#os.system("echo 1 > /sys/devices/soc/6a00000.ssusb/power_supply/usb/usb_otg") |
||||||
|
LEON = True |
||||||
|
bus.close() |
||||||
|
|
||||||
|
last_eon_fan_val = None |
||||||
|
def set_eon_fan(val): |
||||||
|
global LEON, last_eon_fan_val |
||||||
|
|
||||||
|
if last_eon_fan_val is None or last_eon_fan_val != val: |
||||||
|
bus = SMBus(7, force=True) |
||||||
|
if LEON: |
||||||
|
i = [0x1, 0x3 | 0, 0x3 | 0x08, 0x3 | 0x10][val] |
||||||
|
bus.write_i2c_block_data(0x3d, 0, [i]) |
||||||
|
else: |
||||||
|
bus.write_byte_data(0x21, 0x04, 0x2) |
||||||
|
bus.write_byte_data(0x21, 0x03, (val*2)+1) |
||||||
|
bus.write_byte_data(0x21, 0x04, 0x4) |
||||||
|
bus.close() |
||||||
|
last_eon_fan_val = val |
||||||
|
|
||||||
|
# temp thresholds to control fan speed - high hysteresis |
||||||
|
_TEMP_THRS_H = [50., 65., 80., 10000] |
||||||
|
# temp thresholds to control fan speed - low hysteresis |
||||||
|
_TEMP_THRS_L = [42.5, 57.5, 72.5, 10000] |
||||||
|
# fan speed options |
||||||
|
_FAN_SPEEDS = [0, 16384, 32768, 65535] |
||||||
|
# max fan speed only allowed if battery if hot |
||||||
|
_BAT_TEMP_THERSHOLD = 45. |
||||||
|
|
||||||
|
def handle_fan(max_temp, bat_temp, fan_speed): |
||||||
|
new_speed_h = next(speed for speed, temp_h in zip(_FAN_SPEEDS, _TEMP_THRS_H) if temp_h > max_temp) |
||||||
|
new_speed_l = next(speed for speed, temp_l in zip(_FAN_SPEEDS, _TEMP_THRS_L) if temp_l > max_temp) |
||||||
|
|
||||||
|
if new_speed_h > fan_speed: |
||||||
|
# update speed if using the high thresholds results in fan speed increment |
||||||
|
fan_speed = new_speed_h |
||||||
|
elif new_speed_l < fan_speed: |
||||||
|
# update speed if using the low thresholds results in fan speed decrement |
||||||
|
fan_speed = new_speed_l |
||||||
|
|
||||||
|
if bat_temp < _BAT_TEMP_THERSHOLD: |
||||||
|
# no max fan speed unless battery is hot |
||||||
|
fan_speed = min(fan_speed, _FAN_SPEEDS[-2]) |
||||||
|
|
||||||
|
set_eon_fan(fan_speed/16384) |
||||||
|
|
||||||
|
return fan_speed |
||||||
|
|
||||||
|
class LocationStarter(object): |
||||||
|
def __init__(self): |
||||||
|
self.last_good_loc = 0 |
||||||
|
def update(self, started_ts, location): |
||||||
|
rt = sec_since_boot() |
||||||
|
|
||||||
|
if location is None or location.accuracy > 50 or location.speed < 2: |
||||||
|
# bad location, stop if we havent gotten a location in a while |
||||||
|
# dont stop if we're been going for less than a minute |
||||||
|
if started_ts: |
||||||
|
if rt-self.last_good_loc > 60. and rt-started_ts > 60: |
||||||
|
cloudlog.event("location_stop", |
||||||
|
ts=rt, |
||||||
|
started_ts=started_ts, |
||||||
|
last_good_loc=self.last_good_loc, |
||||||
|
location=location.to_dict() if location else None) |
||||||
|
return False |
||||||
|
else: |
||||||
|
return True |
||||||
|
else: |
||||||
|
return False |
||||||
|
|
||||||
|
self.last_good_loc = rt |
||||||
|
|
||||||
|
if started_ts: |
||||||
|
return True |
||||||
|
else: |
||||||
|
cloudlog.event("location_start", location=location.to_dict() if location else None) |
||||||
|
return location.speed*3.6 > 10 |
||||||
|
|
||||||
|
def thermald_thread(): |
||||||
|
setup_eon_fan() |
||||||
|
|
||||||
|
# now loop |
||||||
|
context = zmq.Context() |
||||||
|
thermal_sock = messaging.pub_sock(context, service_list['thermal'].port) |
||||||
|
health_sock = messaging.sub_sock(context, service_list['health'].port) |
||||||
|
location_sock = messaging.sub_sock(context, service_list['gpsLocation'].port) |
||||||
|
fan_speed = 0 |
||||||
|
count = 0 |
||||||
|
|
||||||
|
off_ts = None |
||||||
|
started_ts = None |
||||||
|
ignition_seen = False |
||||||
|
started_seen = False |
||||||
|
passive_starter = LocationStarter() |
||||||
|
thermal_status = ThermalStatus.green |
||||||
|
health_sock.RCVTIMEO = 1500 |
||||||
|
|
||||||
|
params = Params() |
||||||
|
|
||||||
|
while 1: |
||||||
|
td = messaging.recv_sock(health_sock, wait=True) |
||||||
|
location = messaging.recv_sock(location_sock) |
||||||
|
location = location.gpsLocation if location else None |
||||||
|
msg = read_thermal() |
||||||
|
|
||||||
|
# loggerd is gated based on free space |
||||||
|
statvfs = os.statvfs(ROOT) |
||||||
|
avail = (statvfs.f_bavail * 1.0)/statvfs.f_blocks |
||||||
|
|
||||||
|
# thermal message now also includes free space |
||||||
|
msg.thermal.freeSpace = avail |
||||||
|
with open("/sys/class/power_supply/battery/capacity") as f: |
||||||
|
msg.thermal.batteryPercent = int(f.read()) |
||||||
|
with open("/sys/class/power_supply/battery/status") as f: |
||||||
|
msg.thermal.batteryStatus = f.read().strip() |
||||||
|
with open("/sys/class/power_supply/usb/online") as f: |
||||||
|
msg.thermal.usbOnline = bool(int(f.read())) |
||||||
|
|
||||||
|
# TODO: add car battery voltage check |
||||||
|
max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1, |
||||||
|
msg.thermal.cpu2, msg.thermal.cpu3) / 10.0 |
||||||
|
bat_temp = msg.thermal.bat/1000. |
||||||
|
fan_speed = handle_fan(max_temp, bat_temp, fan_speed) |
||||||
|
msg.thermal.fanSpeed = fan_speed |
||||||
|
|
||||||
|
# thermal logic here |
||||||
|
|
||||||
|
if max_temp < 70.0: |
||||||
|
thermal_status = ThermalStatus.green |
||||||
|
if max_temp > 85.0: |
||||||
|
cloudlog.warning("over temp: %r", max_temp) |
||||||
|
thermal_status = ThermalStatus.yellow |
||||||
|
|
||||||
|
# from controls |
||||||
|
overtemp_proc = any(t > 950 for t in |
||||||
|
(msg.thermal.cpu0, msg.thermal.cpu1, msg.thermal.cpu2, |
||||||
|
msg.thermal.cpu3, msg.thermal.mem, msg.thermal.gpu)) |
||||||
|
overtemp_bat = msg.thermal.bat > 60000 # 60c |
||||||
|
if overtemp_proc or overtemp_bat: |
||||||
|
# TODO: hysteresis |
||||||
|
thermal_status = ThermalStatus.red |
||||||
|
|
||||||
|
if max_temp > 107.0 or msg.thermal.bat >= 63000: |
||||||
|
thermal_status = ThermalStatus.danger |
||||||
|
|
||||||
|
# **** starting logic **** |
||||||
|
|
||||||
|
# start constellation of processes when the car starts |
||||||
|
ignition = td is not None and td.health.started |
||||||
|
ignition_seen = ignition_seen or ignition |
||||||
|
|
||||||
|
# add voltage check for ignition |
||||||
|
if not ignition_seen and td is not None and td.health.voltage > 13500: |
||||||
|
ignition = True |
||||||
|
|
||||||
|
do_uninstall = params.get("DoUninstall") == "1" |
||||||
|
accepted_terms = params.get("HasAcceptedTerms") == "1" |
||||||
|
completed_training = params.get("CompletedTrainingVersion") == training_version |
||||||
|
|
||||||
|
should_start = ignition |
||||||
|
|
||||||
|
# have we seen a panda? |
||||||
|
passive = (params.get("Passive") == "1") |
||||||
|
|
||||||
|
# start on gps movement if we haven't seen ignition and are in passive mode |
||||||
|
should_start = should_start or (not (ignition_seen and td) # seen ignition and panda is connected |
||||||
|
and passive |
||||||
|
and passive_starter.update(started_ts, location)) |
||||||
|
|
||||||
|
# with 2% left, we killall, otherwise the phone will take a long time to boot |
||||||
|
should_start = should_start and msg.thermal.freeSpace > 0.02 |
||||||
|
|
||||||
|
# require usb power in passive mode |
||||||
|
should_start = should_start and (not passive or msg.thermal.usbOnline) |
||||||
|
|
||||||
|
# confirm we have completed training and aren't uninstalling |
||||||
|
should_start = should_start and accepted_terms and (passive or completed_training) and (not do_uninstall) |
||||||
|
|
||||||
|
# if any CPU gets above 107 or the battery gets above 63, kill all processes |
||||||
|
# controls will warn with CPU above 95 or battery above 60 |
||||||
|
if msg.thermal.thermalStatus >= ThermalStatus.danger: |
||||||
|
# TODO: Add a better warning when this is happening |
||||||
|
should_start = False |
||||||
|
|
||||||
|
if should_start: |
||||||
|
off_ts = None |
||||||
|
if started_ts is None: |
||||||
|
params.car_start() |
||||||
|
started_ts = sec_since_boot() |
||||||
|
started_seen = True |
||||||
|
else: |
||||||
|
started_ts = None |
||||||
|
if off_ts is None: |
||||||
|
off_ts = sec_since_boot() |
||||||
|
|
||||||
|
# shutdown if the battery gets lower than 3%, it's discharging, we aren't running for |
||||||
|
# more than a minute but we were running |
||||||
|
if msg.thermal.batteryPercent < 3 and msg.thermal.batteryStatus == "Discharging" and \ |
||||||
|
started_seen and (sec_since_boot() - off_ts) > 60: |
||||||
|
os.system('LD_LIBRARY_PATH="" svc power shutdown') |
||||||
|
|
||||||
|
msg.thermal.started = started_ts is not None |
||||||
|
msg.thermal.startedTs = int(1e9*(started_ts or 0)) |
||||||
|
|
||||||
|
msg.thermal.thermalStatus = thermal_status |
||||||
|
thermal_sock.send(msg.to_bytes()) |
||||||
|
print msg |
||||||
|
|
||||||
|
# report to server once per minute |
||||||
|
if (count%60) == 0: |
||||||
|
cloudlog.event("STATUS_PACKET", |
||||||
|
count=count, |
||||||
|
health=(td.to_dict() if td else None), |
||||||
|
location=(location.to_dict() if location else None), |
||||||
|
thermal=msg.to_dict()) |
||||||
|
|
||||||
|
count += 1 |
||||||
|
|
||||||
|
|
||||||
|
def main(gctx=None): |
||||||
|
thermald_thread() |
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
main() |
||||||
|
|
||||||
@ -1,3 +1,3 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
version https://git-lfs.github.com/spec/v1 |
||||||
oid sha256:9cbff07d194fcd129b105b4b9b6aa6a274e130911f10dda614db1ef60092600e |
oid sha256:a21acff35bda607b6780df7730e005a3b289acfc737a1340a7b9f7a3178d83a0 |
||||||
size 13522344 |
size 13965736 |
||||||
|
|||||||
Loading…
Reference in new issue