forked from mnakada/atomcam_tools
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathwait_motion.c
113 lines (100 loc) · 3.55 KB
/
wait_motion.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
#include <stdio.h>
#include <stdlib.h>
#include <dlfcn.h>
#include <string.h>
#include <sys/time.h>
#include <pthread.h>
#include <errno.h>
#include <math.h>
#include <unistd.h>
extern void CommandResponse(int fd, const char *res);
extern int local_sdk_motor_get_position(float *step,float *angle);
extern int MotorFd;
extern struct timeval MotorLastMovedTime;
struct RectInfoSt {
int left;
int right;
int top;
int bottom;
int dummy1;
int dummt2;
};
static int (*original_local_sdk_video_osd_update_rect)(int ch, int display, struct RectInfoSt *rectInfo);
static int WaitMotionFd = -1;
static int Timeout = -1;
static pthread_mutex_t WaitMotionMutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_cond_t WaitMotionCond = PTHREAD_COND_INITIALIZER;
char *WaitMotion(int fd, char *tokenPtr) {
if(WaitMotionFd >= 0) {
fprintf(stderr, "[command] wait motion error %d %d\n", WaitMotionFd, fd);
return "error : wait motion error";
}
char *p = strtok_r(NULL, " \t\r\n", &tokenPtr);
Timeout = p ? atoi(p) : 0;
if(Timeout < 10) {
fprintf(stderr, "[command] wait motion timeout error timeout = %d\n", Timeout);
return "error : wait motion timeout value error";
}
WaitMotionFd = fd;
pthread_mutex_unlock(&WaitMotionMutex);
return NULL;
}
int local_sdk_video_osd_update_rect(int ch, int display, struct RectInfoSt *rectInfo) {
if((WaitMotionFd >= 0) && (MotorFd <= 0) && !ch) {
struct timeval tv;
gettimeofday(&tv, NULL);
timersub(&tv, &MotorLastMovedTime, &tv);
if(tv.tv_sec || (tv.tv_usec >= 500000)) {
if(display) {
float pan; // 0-355
float tilt; // 0-180
int ret = local_sdk_motor_get_position(&pan, &tilt);
static char waitMotionResBuf[256];
if(!ret) {
pan += (rectInfo->left + rectInfo->right - 320 * 2) * 85 / (2 * 640);
if(pan < 0.0) pan = 0.0;
if(pan > 355.0) pan = 355;
tilt -= (rectInfo->top + rectInfo->bottom - 180 * 2) * 55 / (2 * 360);
if(tilt < 45.0) tilt = 45.0;
if(tilt > 180.0) tilt = 180.0;
sprintf(waitMotionResBuf, "detect %d %d %d %d %d %d\n",
rectInfo->left, rectInfo->right, rectInfo->top, rectInfo->bottom, lroundf(pan), lroundf(tilt));
} else {
sprintf(waitMotionResBuf, "detect %d %d %d %d - -\n",
rectInfo->left, rectInfo->right, rectInfo->top, rectInfo->bottom);
}
CommandResponse(WaitMotionFd, waitMotionResBuf);
} else {
CommandResponse(WaitMotionFd, "clear\n");
}
pthread_cond_signal(&WaitMotionCond);
}
}
return original_local_sdk_video_osd_update_rect(ch, display, rectInfo);
}
static void *WaitMotionThread() {
while(1) {
pthread_mutex_lock(&WaitMotionMutex);
if(WaitMotionFd >= 0) {
struct timeval now;
struct timespec timeout;
gettimeofday(&now, NULL);
timeout.tv_sec = now.tv_sec + Timeout;
timeout.tv_nsec = now.tv_usec * 1000;
int ret = pthread_cond_timedwait(&WaitMotionCond, &WaitMotionMutex, &timeout);
if(ret == ETIMEDOUT) CommandResponse(WaitMotionFd, "timeout\n");
}
WaitMotionFd = -1;
}
}
static void __attribute ((constructor)) osd_rect_hook_init(void) {
if(getppid() != 1) return;
original_local_sdk_video_osd_update_rect = dlsym(dlopen ("/system/lib/liblocalsdk.so", RTLD_LAZY), "local_sdk_video_osd_update_rect");
pthread_mutex_lock(&WaitMotionMutex);
pthread_t thread;
if(pthread_create(&thread, NULL, WaitMotionThread, NULL)) {
fprintf(stderr, "pthread_create error\n");
pthread_mutex_unlock(&WaitMotionMutex);
return;
}
}