├── .gitignore ├── LICENSE ├── README.md ├── an41908a ├── Makefile └── an41908a.c ├── api ├── README.md └── architecture.png ├── camhi-motor ├── Readme.md └── main.c ├── doc └── IVG-N83020S-T.pdf ├── i2c-motor ├── Readme.md └── main.c ├── ingenic-motor ├── README.md └── main.c ├── xm-kmotor ├── Makefile ├── Readme.md └── main.c └── xm-uart ├── Makefile ├── Readme.md └── main.c /.gitignore: -------------------------------------------------------------------------------- 1 | *.o 2 | xm-uart-motors-* 3 | xm-kmotor-* 4 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 OpenIPC 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Motors 2 | 3 | Various code to manage motor hardware 4 | 5 | ## Some theory behind 6 | 7 | [Basic autofocus algorithms](https://www.csie.ntu.edu.tw/~fuh/personal/Images&Recognition.Vol.9,No.4.Autofocus.pdf) 8 | 9 | [Learning approach for autofocus](https://openaccess.thecvf.com/content_CVPR_2020/papers/Herrmann_Learning_to_Autofocus_CVPR_2020_paper.pdf) 10 | 11 | [Improving the accuracy and low-light performance of contrast-based autofocus using 12 | supervised machine learning](https://cs.uwaterloo.ca/~vanbeek/Publications/prl-2015.pdf) 13 | 14 | ----- 15 | 16 | ## Various links 17 | 18 | * XM AF Module description [Function Command List on p.6-10](doc/IVG-N83020S-T.pdf) 19 | 20 | * Driver Manuals on stepper motor drivers - http://www.relmon.com/en/index.php/list/13/33.html 21 | 22 | * Some code for ms41929 motor driver - https://download.csdn.net/download/u011212383/10668636 23 | 24 | * Driver for ms32006 - https://github.com/song7788/driver_for_ms32006 25 | 26 | ----- 27 | 28 | ## Similar projects on Github 29 | 30 | * https://github.com/Lynch234ok/git_ipc_hi3516C_V3/tree/master/git_ipc 31 | * https://github.com/stonetan/AF-algorithm 32 | * https://github.com/wztforest/test/tree/236bd04cad7aff12e56b7d26cf785b61a23904c9/ipc/hisi-vsrd/ipcam.hi3518A/src/ptz 33 | -------------------------------------------------------------------------------- /an41908a/Makefile: -------------------------------------------------------------------------------- 1 | MPP_DIR=$(HOME)/projects/cameras/sdk/Hi3516CV500_SDK_V2.0.2.1/smp/a7_linux/mpp 2 | 3 | all: an41908a-orig 4 | 5 | an41908a-orig: CC=arm-himix200-linux-gcc 6 | an41908a-orig: CFLAGS=-I$(MPP_DIR)/include 7 | an41908a-orig: LDFLAGS=-L$(MPP_DIR)/lib 8 | an41908a-orig: LDLIBS=-lisp -l_hildci -l_hidehaze -l_hidrc -l_hiacs -l_hiir_auto -l_hicalcflicker -lmpi -pthread -lsecurec -lVoiceEngine -lupvqe -ldnvqe -lm 9 | 10 | an41908a-%: an41908a.o 11 | $(CC) -o $@ $(LDFLAGS) $(LDLIBS) $^ 12 | sudo cp $@ /mnt/noc/sdk/majestic.cv500 13 | 14 | clean: 15 | -rm -rf an41908a-* *.o 16 | -------------------------------------------------------------------------------- /an41908a/an41908a.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "hi_type.h" 15 | #include "mpi_isp.h" 16 | 17 | #define MAX_LENGTH 64 18 | #define PORT_SPEED 4000000 19 | #define DEVICE_NAME "/dev/spidev2.0" 20 | 21 | #define GPIO_RSTB 27 22 | #define GPIO_VD_IS 19 23 | #define GPIO_VD_FZ 24 24 | 25 | void send_spi(int fd, unsigned char *data, size_t len) { 26 | __u8 miso[MAX_LENGTH]; 27 | struct spi_ioc_transfer tr = { 28 | .tx_buf = (unsigned long)data, 29 | .rx_buf = (unsigned long)miso, 30 | .delay_usecs = 1, 31 | .len = len, 32 | .speed_hz = PORT_SPEED, 33 | }; 34 | int ret = ioctl(fd, SPI_IOC_MESSAGE(1), &tr); 35 | if (ret == -1) { 36 | fprintf(stderr, "main: ioctl SPI_IOC_MESSAGE: %d: %s\n", fd, 37 | strerror(errno)); 38 | } 39 | } 40 | 41 | static bool set_gpio(const int gpio_num, const bool bit) { 42 | char fname[PATH_MAX]; 43 | 44 | char buf = bit ? '1' : '0'; 45 | snprintf(fname, sizeof(fname), "/sys/class/gpio/gpio%d/value", gpio_num); 46 | int fd = open(fname, O_WRONLY); 47 | if (fd < 0) { 48 | fprintf(stderr, "set_gpio(%d, %d) error\n", gpio_num, bit); 49 | return false; 50 | } 51 | write(fd, &buf, 1); 52 | close(fd); 53 | return true; 54 | } 55 | 56 | static void signal_gpio(int gpio_num) { 57 | set_gpio(gpio_num, true); 58 | usleep(10000); 59 | set_gpio(gpio_num, false); 60 | } 61 | 62 | void turn_on(int fd) { 63 | unsigned char cmd[] = {0x23, 0xff, 0xff, 0x28, 0xff, 0xff, 64 | 0x29, 0, 0x5, 0x24, 0, 0x4}; 65 | send_spi(fd, cmd, sizeof(cmd)); 66 | } 67 | 68 | void turn_off(int fd) { 69 | unsigned char cmd[] = {0x23, 0, 0, 0x28, 0, 0, 0x29, 0, 0x4, 0x24, 0, 0x4}; 70 | send_spi(fd, cmd, sizeof(cmd)); 71 | } 72 | 73 | void send_focus_cmd(int fd, bool forward, int step) { 74 | unsigned char cmd[] = {0x29, step, 0x4 + forward, 0x2a, 0x35, 0xc, 75 | 0x24, 0, 0x4, 0x25, 0x38, 0x1, 76 | 0x23, 0xff, 0xff, 0x28, 0xff, 0xff}; 77 | send_spi(fd, cmd, sizeof(cmd)); 78 | } 79 | 80 | u_int16_t get_iris(int fd) { 81 | unsigned char cmd[] = {0x40, 0, 0}; 82 | __u8 miso[MAX_LENGTH]; 83 | struct spi_ioc_transfer tr = { 84 | .tx_buf = (unsigned long)cmd, 85 | .rx_buf = (unsigned long)miso, 86 | .delay_usecs = 1, 87 | .len = sizeof(cmd), 88 | .speed_hz = PORT_SPEED, 89 | }; 90 | int ret = ioctl(fd, SPI_IOC_MESSAGE(1), &tr); 91 | if (ret == -1) { 92 | fprintf(stderr, "main: ioctl SPI_IOC_MESSAGE: %d: %s\n", fd, 93 | strerror(errno)); 94 | } 95 | send_spi(fd, cmd, sizeof(cmd)); 96 | return miso[1] | miso[2] << 8; 97 | } 98 | 99 | static int iris_value; 100 | void set_iris(int fd, unsigned value) { 101 | unsigned char cmd[] = {0x0, value & 0xff, value >> 8}; 102 | send_spi(fd, cmd, sizeof(cmd)); 103 | signal_gpio(GPIO_VD_IS); 104 | iris_value = value; 105 | 106 | printf("set iris %#x, get %#x\n", value, get_iris(fd)); 107 | } 108 | 109 | #define STEP 0x80 110 | 111 | #define IRIS_MIN 0 112 | #define IRIS_MAX 0x03E8 113 | #define IRIS_DEFAULT 0xC8 114 | 115 | void send_iris_cmd(int fd, bool forward) { 116 | int step = forward ? STEP : -STEP; 117 | int nvalue = iris_value + step; 118 | if (nvalue < IRIS_MIN) 119 | nvalue = IRIS_MIN; 120 | if (nvalue > IRIS_MAX) 121 | nvalue = IRIS_MAX; 122 | 123 | set_iris(fd, nvalue); 124 | } 125 | 126 | static void init_iris(int fd) { 127 | unsigned char cmd[] = { 128 | 0x1, 0x8A, 0x7C, 0x2, 0xF0, 0x66, 0x3, 0x10, 0x0E, 0x4, 0xFF, 129 | 0x80, 0x5, 0x24, 0x00, 0xb, 0x80, 0x04, 0xe, 0x00, 0x03, 130 | }; 131 | send_spi(fd, cmd, sizeof(cmd)); 132 | set_iris(fd, IRIS_DEFAULT); 133 | } 134 | 135 | static void init_original(int fd) { 136 | unsigned char cmd1[] = {0x20, 0x1, 0x7f, 0x22, 0x1, 0, 0x27, 0x1, 0, 137 | 0x23, 0xff, 0xff, 0x28, 0xff, 0xff, 0x21, 0x87, 0, 138 | 0x1, 0x8a, 0x7c, 0x2, 0xf0, 0x66, 0x3, 0x10, 0xe, 139 | 0x4, 0xff, 0x80, 0x5, 0x24, 0, 0xb, 0x80, 0x4, 140 | 0xe, 0, 0x3, 0, 0, 0}; 141 | send_spi(fd, cmd1, sizeof(cmd1)); 142 | unsigned char cmd2[] = {0x24, 0, 0xc, 0x29, 0, 0xc}; 143 | send_spi(fd, cmd2, sizeof(cmd2)); 144 | } 145 | 146 | static void init_lens(int fd) { 147 | unsigned char cmd[] = { 148 | 0x20, 0x0A, 0x3C, 0x21, 0x00, 0x00, 0x22, 0x03, 0x00, 149 | 0x23, 0x00, 0x00, 0x27, 0x03, 0x00, 0x28, 0x00, 0x00, 150 | }; 151 | send_spi(fd, cmd, sizeof(cmd)); 152 | } 153 | 154 | void set_focus_precise(int fd, bool forward, int step) { 155 | turn_on(fd); 156 | send_focus_cmd(fd, forward, step); 157 | signal_gpio(GPIO_VD_FZ); 158 | turn_off(fd); 159 | } 160 | 161 | void set_focus(int fd, bool forward) { set_focus_precise(fd, forward, 5); } 162 | 163 | void send_zoom_cmd(int fd, bool forward) { 164 | unsigned char cmd[] = {0x29, 0xff, 0x4 + forward, 0x2a, 0x35, 0xc, 165 | 0x24, 0x50, 0x4 + forward, 0x25, 0x38, 0x1, 166 | 0x23, 0xff, 0xff, 0x28, 0xff, 0xff}; 167 | send_spi(fd, cmd, sizeof(cmd)); 168 | } 169 | 170 | void set_zoom(int fd, bool forward) { 171 | turn_on(fd); 172 | for (int i = 0; i < 20; i++) { 173 | send_zoom_cmd(fd, forward); 174 | signal_gpio(24); 175 | puts("Step"); 176 | } 177 | turn_off(fd); 178 | } 179 | 180 | #define BLEND_SHIFT 6 181 | #define ALPHA 64 // 1 182 | #define BELTA 54 // 0.85 183 | 184 | static const int AFWeight[15][17] = { 185 | {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}, 186 | {1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1}, 187 | {1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1}, 188 | {1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1}, 189 | {1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1}, 190 | {1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1}, 191 | {1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1}, 192 | {1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1}, 193 | {1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1}, 194 | {1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1}, 195 | {1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1}, 196 | {1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1}, 197 | {1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1}, 198 | {1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1}, 199 | {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}, 200 | }; 201 | 202 | static const ISP_FOCUS_STATISTICS_CFG_S gstFocusCfg = { 203 | {1, 204 | #ifdef AF_BLOCK_8X8 205 | 8, 206 | 8, 207 | #else 208 | 17, 209 | 15, 210 | #endif 211 | 1920, 212 | 1080, 213 | 1, 214 | 0, 215 | {0, 0, 0, 1920, 1080}, 216 | 0, 217 | {0x2, 0x4, 0}, 218 | {1, 0x9bff}, 219 | 0xf0}, 220 | {1, 221 | {1, 1, 1}, 222 | 15, 223 | {188, 414, -330, 486, -461, 400, -328}, 224 | {7, 0, 3, 1}, 225 | {1, 0, 255, 0, 220, 8, 14}, 226 | {127, 12, 2047}}, 227 | {0, 228 | {1, 1, 0}, 229 | 2, 230 | {200, 200, -110, 461, -415, 0, 0}, 231 | {6, 0, 1, 0}, 232 | {0, 0, 0, 0, 0, 0, 0}, 233 | {15, 12, 2047}}, 234 | {{20, 16, 0, -16, -20}, {1, 0, 255, 0, 220, 8, 14}, {38, 12, 1800}}, 235 | {{-12, -24, 0, 24, 12}, {1, 0, 255, 0, 220, 8, 14}, {15, 12, 2047}}, 236 | {4, {0, 0}, {1, 1}, 0}}; 237 | 238 | #define SOURCE right 239 | 240 | typedef struct { 241 | int left; 242 | int right; 243 | } pair_t; 244 | 245 | pair_t calculate_fv() { 246 | HI_U32 u32Fv1, u32Fv2; 247 | HI_S32 s32Ret = HI_SUCCESS; 248 | ISP_AF_STATISTICS_S stIspAfStatics; 249 | int ViPipe = 0; 250 | 251 | s32Ret = HI_MPI_ISP_GetVDTimeOut(ViPipe, ISP_VD_FE_START, 5000); 252 | if (HI_SUCCESS != s32Ret) { 253 | printf("HI_MPI_ISP_GetVDTimeOut error!(s32Ret = 0x%x)\n", s32Ret); 254 | return (pair_t){-1, -1}; 255 | } 256 | 257 | s32Ret = HI_MPI_ISP_GetFocusStatistics(ViPipe, &stIspAfStatics); 258 | if (HI_SUCCESS != s32Ret) { 259 | printf("HI_MPI_ISP_GetStatistics error!(s32Ret = 0x%x)\n", s32Ret); 260 | return (pair_t){-1, -1}; 261 | } 262 | 263 | HI_U32 i, j; 264 | HI_U32 u32SumFv1 = 0; 265 | HI_U32 u32SumFv2 = 0; 266 | HI_U32 u32WgtSum = 0; 267 | HI_U32 u32Fv1_n, u32Fv2_n; 268 | for (i = 1; i < gstFocusCfg.stConfig.u16Vwnd; i++) { 269 | for (j = 1; j < gstFocusCfg.stConfig.u16Hwnd; j++) { 270 | HI_U32 u32H1 = stIspAfStatics.stBEAFStat.stZoneMetrics[i][j].u16h1; 271 | HI_U32 u32H2 = stIspAfStatics.stBEAFStat.stZoneMetrics[i][j].u16h2; 272 | HI_U32 u32V1 = stIspAfStatics.stBEAFStat.stZoneMetrics[i][j].u16v1; 273 | HI_U32 u32V2 = stIspAfStatics.stBEAFStat.stZoneMetrics[i][j].u16v2; 274 | u32Fv1_n = 275 | (u32H1 * ALPHA + u32V1 * ((1 << BLEND_SHIFT) - ALPHA)) >> BLEND_SHIFT; 276 | u32Fv2_n = 277 | (u32H2 * BELTA + u32V2 * ((1 << BLEND_SHIFT) - BELTA)) >> BLEND_SHIFT; 278 | 279 | u32SumFv1 += AFWeight[i][j] * u32Fv1_n; 280 | u32SumFv2 += AFWeight[i][j] * u32Fv2_n; 281 | u32WgtSum += AFWeight[i][j]; 282 | } 283 | } 284 | 285 | u32Fv1 = u32SumFv1 / u32WgtSum; 286 | u32Fv2 = u32SumFv2 / u32WgtSum; 287 | 288 | printf("u32Fv1=%4d u32Fv2=%4d\n", u32Fv1, u32Fv2); 289 | 290 | return (pair_t){u32Fv1, u32Fv2}; 291 | } 292 | 293 | int exit_AF = 0; 294 | int fd = -1; 295 | 296 | // Basic Autofocus algorithm: 297 | // Stage 1: find starting direction from current point: step right, check 298 | // that it's the right direction, otherwise change direction 299 | // Stage 2: find maximum towards direction 300 | // Stage 3: stop at maximum when well enough 301 | void *AF_proc(void *arg) { 302 | int maxValue = 0; 303 | int cntMissed = 0, maxMissed = 4; 304 | 305 | // 1. figure out current focus values 306 | pair_t initial = calculate_fv(); 307 | 308 | // 2. move focus to another point as much as possible 309 | set_focus_precise(fd, true, 0x50); 310 | sleep(2); 311 | 312 | // 3. figure out values of next step 313 | pair_t current = calculate_fv(); 314 | 315 | if (current.SOURCE == initial.SOURCE) { 316 | puts("Nothing to do, unknown dirrection. Blurry image?"); 317 | goto quit; 318 | } 319 | 320 | bool direction = current.left > initial.left; 321 | printf("DIRECTION %d\n", direction); 322 | 323 | int step = 0x16; 324 | 325 | while (exit_AF == 0) { 326 | set_focus_precise(fd, direction, step); 327 | usleep(200000); 328 | 329 | current = calculate_fv(); 330 | 331 | if (current.SOURCE > maxValue) { 332 | maxValue = current.SOURCE; 333 | } else { 334 | cntMissed++; 335 | direction = !direction; 336 | printf("DIRECTION %d, step %#X\n", direction, step); 337 | if (cntMissed == maxMissed) { 338 | cntMissed = 0; 339 | step /= 2; 340 | maxValue = 0; 341 | maxMissed--; 342 | if (step == 0 || maxMissed == 0) { 343 | puts("Out"); 344 | break; 345 | } 346 | } 347 | } 348 | } 349 | 350 | quit: 351 | // ensure thread stopped 352 | exit_AF = 1; 353 | return ((void *)0); 354 | } 355 | 356 | pthread_t g_af_thread; 357 | static void toggle_af(void) { 358 | if (exit_AF == 0 && g_af_thread) { 359 | exit_AF = 1; 360 | pthread_join(g_af_thread, NULL); 361 | g_af_thread = 0; 362 | return; 363 | } 364 | 365 | exit_AF = 0; 366 | pthread_attr_t attr1; 367 | pthread_attr_init(&attr1); 368 | pthread_attr_setstacksize(&attr1, 0x10000); 369 | pthread_create(&g_af_thread, &attr1, AF_proc, NULL); 370 | } 371 | 372 | static void setup_gpio(int gpio_num, bool output) { 373 | char fname[PATH_MAX]; 374 | snprintf(fname, sizeof(fname), "/sys/class/gpio/gpio%d", gpio_num); 375 | if (access(fname, 0)) { 376 | int fd = open("/sys/class/gpio/export", O_WRONLY); 377 | if (fd < 0) { 378 | fprintf(stderr, "Cannot export GPIO %d\n", gpio_num); 379 | return; 380 | } 381 | 382 | char num[16]; 383 | snprintf(num, sizeof(num), "%d", gpio_num); 384 | write(fd, num, sizeof(num) - 1); 385 | close(fd); 386 | } 387 | 388 | snprintf(fname, sizeof(fname), "/sys/class/gpio/gpio%d/direction", gpio_num); 389 | int fd = open(fname, O_RDWR); 390 | if (fd < 0) { 391 | fprintf(stderr, "Cannot switch GPIO direction %d\n", gpio_num); 392 | return; 393 | } 394 | 395 | const char *direction = output ? "out" : "in"; 396 | size_t dirlen = strlen(direction); 397 | 398 | char buf[32] = {0}; 399 | if (read(fd, buf, sizeof(buf)) <= 0) { 400 | fprintf(stderr, "Cannot read GPIO direction %d\n", gpio_num); 401 | return; 402 | } 403 | if (memcmp(buf, direction, dirlen) == 0) { 404 | return; 405 | } 406 | 407 | write(fd, direction, dirlen); 408 | close(fd); 409 | } 410 | 411 | static void export_gpios() { 412 | setup_gpio(GPIO_RSTB, true); 413 | set_gpio(GPIO_RSTB, 1); 414 | usleep(10000); 415 | set_gpio(GPIO_RSTB, 0); 416 | usleep(10000); 417 | set_gpio(GPIO_RSTB, 1); 418 | 419 | setup_gpio(GPIO_VD_FZ, true); 420 | setup_gpio(GPIO_VD_IS, true); 421 | } 422 | 423 | int main(int argc, char *argv[]) { 424 | int ret; 425 | 426 | struct termios ctrl; 427 | tcgetattr(STDIN_FILENO, &ctrl); 428 | ctrl.c_lflag &= ~ICANON; // turning off canonical mode makes input unbuffered 429 | ctrl.c_lflag &= ~ECHO; // disable echo 430 | ctrl.c_lflag &= ~ISIG; // disable system Ctrl-C 431 | tcsetattr(STDIN_FILENO, TCSANOW, &ctrl); 432 | 433 | fd = open("/dev/spidev2.0", O_RDWR); 434 | if (fd == -1) { 435 | fprintf(stderr, "main: opening device file: %s: %s\n", DEVICE_NAME, 436 | strerror(errno)); 437 | return -1; 438 | } 439 | 440 | int options = SPI_CPHA | SPI_CPOL | SPI_LSB_FIRST | SPI_LSB_FIRST; 441 | ret = ioctl(fd, SPI_IOC_WR_MODE, &options); 442 | if (ret < 0) { 443 | printf("ioctl SPI_IOC_WR_MODE err, value = %d ret = %d\n", options, ret); 444 | return ret; 445 | } 446 | 447 | int value = 8; 448 | ret = ioctl(fd, SPI_IOC_WR_BITS_PER_WORD, &value); 449 | if (ret < 0) { 450 | printf("ioctl SPI_IOC_WR_BITS_PER_WORD err, value = %d ret = %d\n", value, 451 | ret); 452 | return ret; 453 | } 454 | 455 | value = PORT_SPEED; 456 | ret = ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, &value); 457 | if (ret < 0) { 458 | printf("ioctl SPI_IOC_WR_MAX_SPEED_HZ err, value = %d ret = %d\n", value, 459 | ret); 460 | return ret; 461 | } 462 | 463 | export_gpios(); 464 | init_iris(fd); 465 | init_lens(fd); 466 | // init_original(fd); 467 | 468 | printf("AN41908A controller\n"); 469 | printf("Commands:\n+ - (Zoom) z x (Focus) a (toggle AF) q w (Iris)\n"); 470 | 471 | while (1) { 472 | char ch; 473 | int i = read(0, &ch, 1); 474 | 475 | if (!i) { 476 | printf("stdin closed\n"); 477 | return 0; 478 | } 479 | 480 | switch (ch) { 481 | case '-': 482 | puts("Zoom out"); 483 | set_zoom(fd, true); 484 | break; 485 | 486 | case '+': 487 | puts("Zoom in"); 488 | set_zoom(fd, false); 489 | break; 490 | 491 | case 'z': 492 | puts("Focus near"); 493 | set_focus_precise(fd, false,0xff); 494 | break; 495 | 496 | case 'x': 497 | puts("Focus far"); 498 | set_focus_precise(fd, true,0xff); 499 | break; 500 | 501 | case 'a': 502 | toggle_af(); 503 | break; 504 | 505 | case 'q': 506 | send_iris_cmd(fd, true); 507 | break; 508 | 509 | case 'w': 510 | send_iris_cmd(fd, false); 511 | break; 512 | 513 | case 3: 514 | goto outer; 515 | 516 | default: 517 | printf("Unknown command %c\n", ch); 518 | } 519 | } 520 | 521 | outer: 522 | turn_off(fd); 523 | close(fd); 524 | 525 | return ret; 526 | } 527 | -------------------------------------------------------------------------------- /api/README.md: -------------------------------------------------------------------------------- 1 | ## Thoughts on the structure of _motors API_. 2 | 3 | ### Brief overview 4 | Motors-daemon --- a separate motors-handling daemon with different backends (_driver-wrappers_). 5 | Backends are pluggable extensions which add platform/protocol-specific functionality such as UART protocol, an41908a-based, CamHi-based controls. 6 | The daemon unites them and exposes unified API for a camera applications. 7 | 8 | ### Abstraction layers 9 | 1. #### _Device drivers_ (kernel modules) 10 | Drivers create a device file like `/dev/motor` encapsulate timers/phase/gpio control logic (questionable, shouldn't it be implemented at a user-space?). Kernel module files can be taken from an original firmware or compiled from source. 11 | 12 | 2. #### _Driver wrappers_ (libraries) 13 | Driver-specific, small programs which proxy function calls and **provide the standardized interface** to higher-level programs (`Adapter pattern`). 14 | They are served with corresponding _device-drivers_ and usually wrap `ioctl` calls. But in some cases (such as _PELCO_ protocol in [xm-uart/main.c](https://github.com/OpenIPC/motors/blob/5993229147e3631490a3addd14803946bb7b5df7/xm-uart/main.c)) they encapsulate the details of protocol implementation. 15 | 16 | "User" can write his own _wrapper_ and plug it into the _motors-daemon_ to add support for new _device driver_. 17 | 18 | 3. #### _Motors-daemon_ 19 | The core of the motors API. It runs as a daemon and listens to commands via `UNIX socket`. 20 | Other applications connect to the socket and send _Pan/Tilt/Zoom/Focus/IRCut/AF_ commands (The full list of supported commands should be defined in the API specification). Applications may also want to get info about day/night state, current preset etc. 21 | User should register a _device-wrapper_ by writing a path to the suitable _driver-wrapper_ in a config file. 22 | 23 | ![Service architecture](./architecture.png) 24 | 25 | ### Daemon responsibilities 26 | 1. Read config which contains default params and a path to _wrapper library_. 27 | 2. Load wrapper as `.so` library. 28 | 3. Call initialization function of the _wrapper library_. 29 | 3. Translate socket requests to library calls. 30 | 4. Return states of connected peripherals. 31 | 32 | ### List of capabilities 33 | #### Controls 34 | - PTZ 35 | - AF (start, stop) 36 | - IRCut 37 | - IR backlight 38 | - set/call preset 39 | - calibrate max/min motor positions 40 | - set step size 41 | #### States 42 | - get day/night state 43 | - get presets 44 | - get max/min motor positions 45 | #### Response status 46 | - functionality not supported 47 | - invalid command 48 | - invocation failed 49 | - success 50 | 51 | ### Roadmap 52 | 53 | - [ ] Decide on unified API for _driver wrappers_. The list of functions they provide to motors-daemon. 54 | - [ ] Adjust existing _wrappers'_ signatures ([xm-kmotor/main.c](https://github.com/OpenIPC/motors/blob/5993229147e3631490a3addd14803946bb7b5df7/xm-kmotor/main.c) and others) according to the API. 55 | - [ ] Decide on motors-daemon _Unix sockets_ API. 56 | - [ ] Implement motors-daemon as a server that listens to socket connections. 57 | - [ ] Implement motors-daemon's plug-in logic to call functions of the provided _driver-wrapper_. 58 | 59 | ### Problems 60 | 1. Should the daemon use only one _driver-wrapper_ at a time (specified in the config) or it can combine drivers with different functionality (for example, one stands for PTZ and other manages IRCut)? 61 | 2. On which layer the protocol-specific logic should live - _kernel module_ or _driver wrapper_? 62 | - A kernel module should be as lightweight as possible and provide basic functionality via `ioctl` or just create a device and do _pinmuxing_. 63 | - Driver-wrapper (library) is a thin adapter layer between a _hardware driver_ and the _daemon_. 64 | So, theoretically it should NOT implement any protocol-specific logic. 65 | But in practice the protocol depends on a board and implemented inside _driver-wrapper_ (like _PELCO_). 66 | 3. Also, when adding new gpio motors, for example, should we write a user-space library (_driver-wrapper_) and skip the device driver layer (because linux already provides a way to manage gpio without custom kernel module)? 67 | Or place the logic inside a new kernel module and leave the _driver-wrapper_ as thin as possible? 68 | -------------------------------------------------------------------------------- /api/architecture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenIPC/motors/e618f713395990d5f9e4815dcc159477ac413a58/api/architecture.png -------------------------------------------------------------------------------- /camhi-motor/Readme.md: -------------------------------------------------------------------------------- 1 | ## Example usage 2 | 3 | ``` 4 | insmod /lib/modules/4.9.37/hisilicon/camhi-motor.ko 5 | ``` 6 | 7 | ``` 8 | -d Direction step ( u (Zoom+), d (Zoom-), l (Focus-), r (Focus+), s (Stop)) 9 | -s Speed step (default 5) 10 | ``` 11 | 12 | ``` 13 | camhi-motor -d r -s 50 14 | camhi-motor -d l -s 50 15 | camhi-motor -d d -s 50 16 | camhi-motor -d u -s 50 17 | ``` 18 | 19 | 20 | -------------------------------------------------------------------------------- /camhi-motor/main.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #define MOTOR_STATUS 0xC0046D02 13 | #define MOTOR_CMD 0xC0046D01 14 | 15 | 16 | 17 | void motor_ioctl(int cmd, int *arg) { 18 | int fd = open("/dev/motor", O_WRONLY); 19 | ioctl(fd, cmd, arg); 20 | close(fd); 21 | } 22 | 23 | void motor_status_get(int *cmd) { 24 | motor_ioctl(MOTOR_STATUS, cmd); 25 | } 26 | 27 | void sendCommand(int cmd, int sspeed) { 28 | int s[3]; 29 | s[0] = cmd; 30 | s[1] = sspeed; //xspeed 31 | s[2] = sspeed; //yspeed 32 | motor_ioctl(MOTOR_CMD, s); 33 | 34 | // show_sage(); 35 | } 36 | 37 | int main (int argc, char *argv[]) { 38 | int steps[5]; 39 | char direction = 's'; 40 | int stepspeed = 20; 41 | int xpos = 0; 42 | int ypos = 0; 43 | int c; 44 | //sendCommand(0, stepspeed); //Stop 45 | //sendCommand(1, stepspeed); //Zoom- 46 | //sendCommand(2, stepspeed); //Zoom+ 47 | //sendCommand(3, stepspeed); //Focus+ 48 | //sendCommand(4, stepspeed); //Focus- 49 | 50 | while ((c = getopt(argc, argv, "d:s:")) != -1) { 51 | switch (c) { 52 | case 'd': 53 | direction = optarg[0]; 54 | break; 55 | case 's': 56 | if (atoi(optarg) > 100) 57 | { 58 | stepspeed = 10; 59 | }else { 60 | stepspeed =atoi(optarg); 61 | } 62 | 63 | break; 64 | default: 65 | printf("Invalid Argument %c\n", c); 66 | printf("Usage : %s\n" 67 | "\t -d Direction step\n" 68 | "\t -s Speed step (default 5)\n" , argv[0]); 69 | exit(EXIT_FAILURE); 70 | } 71 | } 72 | 73 | switch (direction) { 74 | case 'u': //Zoom+ 75 | sendCommand(2, stepspeed); 76 | break; 77 | case 'd': //Zoom- 78 | sendCommand(1, stepspeed); 79 | break; 80 | case 'l': //Focus- 81 | sendCommand(4, stepspeed); 82 | break; 83 | case 'r': //Focus+ 84 | sendCommand(3, stepspeed); 85 | break; 86 | case 's': //stop 87 | sendCommand(0, stepspeed); 88 | break; 89 | 90 | default: 91 | printf("Invalid Direction Argument %c\n", direction); 92 | printf("Usage : %s -d\n" 93 | "\t u (Zoom+)\n" 94 | "\t d (Zoom-)\n" 95 | "\t l (Focus-)\n" 96 | "\t r (Focus+)\n" 97 | "\t s (Stop)\n", argv[0]); 98 | exit(EXIT_FAILURE); 99 | } 100 | /* sendCommand(3, stepspeed); 101 | 102 | motor_status_get(steps); 103 | printf("Status Move: %d.\n", steps[0]); 104 | printf("X Steps %d.\n", steps[1]); 105 | printf("Y Steps %d.\n", steps[2]); */ 106 | return 0; 107 | } 108 | -------------------------------------------------------------------------------- /doc/IVG-N83020S-T.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenIPC/motors/e618f713395990d5f9e4815dcc159477ac413a58/doc/IVG-N83020S-T.pdf -------------------------------------------------------------------------------- /i2c-motor/Readme.md: -------------------------------------------------------------------------------- 1 | ## Example usage 2 | 3 | ``` 4 | devmem 0x112C0058 32 0x1032 # Set I2C2_SCL, connected to GPIO7_0 (gpio56) 5 | devmem 0x112C005C 32 0x1032 # Set I2C2_SDA, connected to GPIO7_1 (gpio57) 6 | devmem 0x100C000C 32 0x1 7 | devmem 0x120101BC 32 0x801282 8 | ``` 9 | 10 | ``` 11 | -d Direction step ( i (Init), u (Zoom+), d (Zoom-), l (Focus-), r (Focus+), s (Stop)) 12 | -s Speed step (default 5) 13 | ``` 14 | 15 | ``` 16 | i2c-motor -d i # init motors 17 | i2c-motor -d r -s 50 18 | i2c-motor -d l -s 50 19 | i2c-motor -d d -s 50 20 | i2c-motor -d u -s 50 21 | i2c-motor -d s # stop motors 22 | ``` 23 | 24 | 25 | -------------------------------------------------------------------------------- /i2c-motor/main.c: -------------------------------------------------------------------------------- 1 | /* 2 | * I2C Motor driver 3 | * 4 | * Copyright 2022 Aleksander Volkov 5 | * 6 | * This program is free software; you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation; either version 2 of the License, or 9 | * (at your option) any later version. 10 | */ 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | static inline __s32 i2c_smbus_access(int file, char read_write, __u8 command, 22 | int size, union i2c_smbus_data *data) 23 | { 24 | struct i2c_smbus_ioctl_data args; 25 | 26 | args.read_write = read_write; 27 | args.command = command; 28 | args.size = size; 29 | args.data = data; 30 | return ioctl(file,I2C_SMBUS,&args); 31 | } 32 | 33 | 34 | static inline __s32 i2c_smbus_read_byte_data(int file, __u8 command) 35 | { 36 | union i2c_smbus_data data; 37 | if (i2c_smbus_access(file,I2C_SMBUS_READ,command, 38 | I2C_SMBUS_BYTE_DATA,&data)) 39 | return -1; 40 | else 41 | return 0x0FF & data.byte; 42 | } 43 | 44 | 45 | static inline __s32 i2c_smbus_write_byte_data(int file, __u8 command, __u8 value) 46 | { 47 | union i2c_smbus_data data; 48 | data.byte = value; 49 | return i2c_smbus_access(file, I2C_SMBUS_WRITE, command, 50 | I2C_SMBUS_BYTE_DATA, &data); 51 | } 52 | 53 | 54 | void initMotor() 55 | { 56 | uint8_t data, addr = 0x10; 57 | const char *path = "/dev/i2c-2"; 58 | int fd, rc; 59 | 60 | int v3; // r4 61 | int v4; // r0 62 | int v5; // r8 63 | int v7; // r1 64 | int v8[10]; // [sp+0h] [bp-28h] BYREF 65 | 66 | v3 = 3; 67 | while ( 1 ) 68 | { 69 | usleep(0x7A120u); 70 | v4 = open("/dev/pwm", 0); 71 | v5 = v4; 72 | if ( v4 >= 0 ) 73 | break; 74 | if ( !--v3 ) 75 | { 76 | puts("open pwm fails. "); 77 | } 78 | } 79 | v8[0] = 0; 80 | v8[1] = 1; 81 | v8[3] = 1; 82 | v8[2] = 2; 83 | v7 = ioctl(v4, 1u, v8); 84 | if ( v7 ) 85 | printf("HI_PWM_Ctrl: PWM_CMD_WRITE failed: %d\n", v7); 86 | close(v5); 87 | 88 | fd = open(path, O_RDWR); 89 | rc = ioctl(fd, I2C_SLAVE, addr); 90 | 91 | data = i2c_smbus_write_byte_data(fd, 0x00, 0x01); 92 | data = i2c_smbus_write_byte_data(fd, 0x01, 0x52); 93 | data = i2c_smbus_write_byte_data(fd, 0x02, 0x80); 94 | data = i2c_smbus_write_byte_data(fd, 0x03, 0x08); 95 | data = i2c_smbus_write_byte_data(fd, 0x04, 0x00); 96 | data = i2c_smbus_write_byte_data(fd, 0x05, 0x52); 97 | data = i2c_smbus_write_byte_data(fd, 0x06, 0x80); 98 | data = i2c_smbus_write_byte_data(fd, 0x07, 0x08); 99 | data = i2c_smbus_write_byte_data(fd, 0x08, 0x00); 100 | data = i2c_smbus_write_byte_data(fd, 0x09, 0x0f); 101 | data = i2c_smbus_write_byte_data(fd, 0x0a, 0x08); 102 | 103 | close(fd); 104 | } 105 | 106 | 107 | void sendCommand(int cmd, int sspeed) 108 | { 109 | uint8_t data, addr = 0x10, reg = 0x00, value = 0x00; 110 | const char *path = "/dev/i2c-2"; 111 | int fd,fp, rc,rp,sa,sb,s; 112 | 113 | s = sspeed*20; 114 | sb = s / 256; 115 | sa = s - sb * 256; 116 | 117 | fd = open(path, O_RDWR); 118 | rc = ioctl(fd, I2C_SLAVE, addr); 119 | switch (cmd) { 120 | case 0: //stop 121 | data = i2c_smbus_write_byte_data(fd, 0x09, 0x07); 122 | data = i2c_smbus_write_byte_data(fd, 0x00, 0x07); 123 | data = i2c_smbus_write_byte_data(fd, 0x00, 0x00); 124 | data = i2c_smbus_write_byte_data(fd, 0x00, 0x01); 125 | break; 126 | case 1: //Zoom- 127 | data = i2c_smbus_write_byte_data(fd, 0x03, sa); 128 | data = i2c_smbus_write_byte_data(fd, 0x04, 0xc0 + sb); 129 | data = i2c_smbus_write_byte_data(fd, 0x09, 0x8f); 130 | break; 131 | case 2: //Zoom+ 132 | data = i2c_smbus_write_byte_data(fd, 0x03, sa); 133 | data = i2c_smbus_write_byte_data(fd, 0x04, 0xb0 + sb); 134 | data = i2c_smbus_write_byte_data(fd, 0x09, 0x8f); 135 | break; 136 | case 3: //Focus- 137 | data = i2c_smbus_write_byte_data(fd, 0x07, sa); 138 | data = i2c_smbus_write_byte_data(fd, 0x08, 0xc0 + sb); 139 | data = i2c_smbus_write_byte_data(fd, 0x09, 0x4f); 140 | break; 141 | case 4: //Focus+ 142 | data = i2c_smbus_write_byte_data(fd, 0x07, sa); 143 | data = i2c_smbus_write_byte_data(fd, 0x08, 0xb0 + sb); 144 | data = i2c_smbus_write_byte_data(fd, 0x09, 0x4f); 145 | break; 146 | } 147 | close(fd); 148 | } 149 | 150 | int main(int argc, char **argv) 151 | { 152 | uint8_t data, addr = 0x10, reg = 0x00, value = 0x00; 153 | const char *path = "/dev/i2c-2"; 154 | int file, rc; 155 | int steps[5]; 156 | char direction = 's'; 157 | int stepspeed = 5; 158 | int c; 159 | //sendCommand(0, stepspeed); //Stop 160 | //sendCommand(1, stepspeed); //Zoom- 161 | //sendCommand(2, stepspeed); //Zoom+ 162 | //sendCommand(3, stepspeed); //Focus+ 163 | //sendCommand(4, stepspeed); //Focus- 164 | 165 | while ((c = getopt(argc, argv, "d:s:")) != -1) { 166 | switch (c) { 167 | case 'd': 168 | direction = optarg[0]; 169 | break; 170 | case 's': 171 | if (atoi(optarg) > 100) 172 | { 173 | stepspeed = 10; 174 | }else { 175 | stepspeed =atoi(optarg); 176 | } 177 | 178 | break; 179 | default: 180 | printf("Invalid Argument %c\n", c); 181 | printf("Usage : %s\n" 182 | "\t -d Direction step\n" 183 | "\t -s Speed step (default 5)\n" , argv[0]); 184 | exit(EXIT_FAILURE); 185 | } 186 | } 187 | 188 | switch (direction) { 189 | case 'u': //Zoom+ 190 | sendCommand(1, stepspeed); 191 | break; 192 | case 'd': //Zoom- 193 | sendCommand(2, stepspeed); 194 | break; 195 | case 'l': //Focus- 196 | sendCommand(4, stepspeed); 197 | break; 198 | case 'r': //Focus+ 199 | sendCommand(3, stepspeed); 200 | break; 201 | case 's': //stop 202 | sendCommand(0, stepspeed); 203 | break; 204 | case 'i': //stop 205 | initMotor(); 206 | break; 207 | default: 208 | printf("Invalid Direction Argument %c\n", direction); 209 | printf("Usage : %s -d\n" 210 | "\t i (Init)\n" 211 | "\t u (Zoom+)\n" 212 | "\t d (Zoom-)\n" 213 | "\t l (Focus-)\n" 214 | "\t r (Focus+)\n" 215 | "\t s (Stop)\n", argv[0]); 216 | exit(EXIT_FAILURE); 217 | } 218 | return 0; 219 | } 220 | -------------------------------------------------------------------------------- /ingenic-motor/README.md: -------------------------------------------------------------------------------- 1 | 2 | ## Load kernel module before use 3 | ingenic-motor is a command line tool to be able to send commands to the motor.ko camera module. By default this module is not loaded and so it is necessary to enter the following commands: 4 | 5 | ``` 6 | modprobe motor hmaxstep=2540 vmaxstep=720 hst1=52 hst2=53 hst3=57 hst4=51 vst1=59 vst2=61 vst3=62 vst4=63 7 | ``` 8 | 9 | To automate this process during boot, add the line `motor hmaxstep=2540 vmaxstep=720 hst1=52 hst2=53 hst3=57 hst4=51 vst1=59 vst2=61 vst3=62 vst4=63` to `/etc/modules`. 10 | 11 | ## Module Configuration 12 | 13 | - `hstX`: Horizontal motor phase GPIO pins. 14 | - `vstX`: Vertical motor phase GPIO pins. 15 | - `hmaxstep` and `vmaxstep`: Specify the maximum number of steps your hardware can handle. 16 | Note that the maximum steps for the horizontal and vertical motors are passed as arguments when inserting the `motor` module. 17 | 18 | ### Examples for Wyze Pan Cam v3 19 | 20 | 1) connect to the camera via ssh 21 | ``` 22 | ssh root@ip.of.your.camera 23 | ``` 24 | 2) load the kernel module: 25 | 26 | check if the motor module is loaded: 27 | ``` 28 | lsmod 29 | ``` 30 | if any of them are on the list then unload them first: 31 | ``` 32 | rmmod motor 33 | ``` 34 | load the modules with parameters (you may need to experiment with the hmaxstep and vmaxstep values for your specific camera): 35 | 36 | ``` 37 | insmod /path/to/motor.ko hmaxstep=2130 vmaxstep=1600 38 | ``` 39 | 40 | 3) testing 41 | 42 | By passing the -S command line argument, the current status and x,y parameters will be returned: 43 | ``` 44 | ingenic-motor -S 45 | ``` 46 | it will look like this: 47 | ``` 48 | Max X Steps 2130. 49 | Max Y Steps 1600. 50 | Status Move: 0. 51 | X Steps 1065. 52 | Y Steps 800. 53 | Speed 900. 54 | ``` 55 | Note: Seems like `900` is the maximum speed of the kernel module, hence why it can't be set further than that value. 56 | 57 | ### Command line options 58 | ``` 59 | Usage : ingenic-motor 60 | -d Direction step. 61 | -s Speed step (default 900). 62 | -x X position/step (default 0). 63 | -y Y position/step (default 0). 64 | -r reset to default pos. 65 | -j return json string xpos,ypos,status,speed. 66 | -i return json string for all camera parameters 67 | -S show status 68 | ``` 69 | 70 | ## Examples 71 | 72 | * go to mid position of X and Y (assuming max X steps 2130 and max y steps 1600): 73 | ``` 74 | ingenic-motor -d t -x 1065 -y 800 75 | ``` 76 | * go to position of begining of X and Y 77 | ``` 78 | ingenic-motor -d h -x 0 -y 0 79 | ``` 80 | * go to x 1065 and y 0 81 | ``` 82 | ingenic-motor -d h -x 1992 -y 0 83 | ``` 84 | * get camera details as json string 85 | ``` 86 | ingenic-motor -i 87 | ``` 88 | * stop the motors 89 | ``` 90 | ingenic-motor -d s 91 | ``` 92 | * reset the motors (to the center) 93 | ``` 94 | ingenic-motor -r 95 | ``` 96 | -------------------------------------------------------------------------------- /ingenic-motor/main.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #define MOTOR_MOVE_STOP 0x0 13 | #define MOTOR_MOVE_RUN 0x1 14 | 15 | /* directional_attr */ 16 | #define MOTOR_DIRECTIONAL_UP 0x0 17 | #define MOTOR_DIRECTIONAL_DOWN 0x1 18 | #define MOTOR_DIRECTIONAL_LEFT 0x2 19 | #define MOTOR_DIRECTIONAL_RIGHT 0x3 20 | 21 | #define MOTOR1_MAX_SPEED 1000 22 | #define MOTOR1_MIN_SPEED 10 23 | 24 | /* ioctl cmd */ 25 | #define MOTOR_STOP 0x1 26 | #define MOTOR_RESET 0x2 27 | #define MOTOR_MOVE 0x3 28 | #define MOTOR_GET_STATUS 0x4 29 | #define MOTOR_SPEED 0x5 30 | #define MOTOR_GOBACK 0x6 31 | #define MOTOR_CRUISE 0x7 32 | 33 | enum motor_status 34 | { 35 | MOTOR_IS_STOP, 36 | MOTOR_IS_RUNNING, 37 | }; 38 | 39 | struct motor_message 40 | { 41 | int x; 42 | int y; 43 | enum motor_status status; 44 | int speed; 45 | /* these two members are not standard from the original kernel module */ 46 | unsigned int x_max_steps; 47 | unsigned int y_max_steps; 48 | }; 49 | 50 | struct motors_steps 51 | { 52 | int x; 53 | int y; 54 | }; 55 | 56 | struct motor_move_st 57 | { 58 | int motor_directional; 59 | int motor_move_steps; 60 | int motor_move_speed; 61 | }; 62 | struct motor_status_st 63 | { 64 | int directional_attr; 65 | int total_steps; 66 | int current_steps; 67 | int min_speed; 68 | int cur_speed; 69 | int max_speed; 70 | int move_is_min; 71 | int move_is_max; 72 | }; 73 | 74 | struct motor_reset_data 75 | { 76 | unsigned int x_max_steps; 77 | unsigned int y_max_steps; 78 | unsigned int x_cur_step; 79 | unsigned int y_cur_step; 80 | }; 81 | 82 | int fd = -1; 83 | 84 | void motor_ioctl(int cmd, void *arg) 85 | { 86 | // printf("[IOCTL] %d, %p\n", cmd, arg); 87 | ioctl(fd, cmd, arg); 88 | } 89 | 90 | void motor_status_get(struct motor_message *msg) 91 | { 92 | motor_ioctl(MOTOR_GET_STATUS, msg); 93 | } 94 | 95 | void motor_get_maxsteps(unsigned int *maxx, unsigned int *maxy) 96 | { 97 | struct motor_message msg; 98 | motor_status_get(&msg); 99 | if (maxx) 100 | *maxx = msg.x_max_steps; 101 | if (maxy) 102 | *maxy = msg.y_max_steps; 103 | } 104 | 105 | int motor_is_busy() 106 | { 107 | struct motor_message msg; 108 | motor_status_get(&msg); 109 | return msg.status == MOTOR_IS_RUNNING ? 1 : 0; 110 | } 111 | 112 | void motor_wait_idle() 113 | { 114 | while (motor_is_busy()) 115 | { 116 | usleep(100000); 117 | } 118 | printf(" == moving, waiting...\n"); 119 | } 120 | 121 | void motor_steps(int xsteps, int ysteps, int stepspeed) 122 | { 123 | struct motors_steps steps; 124 | steps.x = xsteps; 125 | steps.y = ysteps; 126 | 127 | printf(" -> steps, X %d, Y %d, speed %d\n", steps.x, steps.y, stepspeed); 128 | motor_ioctl(MOTOR_SPEED, &stepspeed); 129 | motor_ioctl(MOTOR_MOVE, &steps); 130 | 131 | motor_wait_idle(); 132 | } 133 | 134 | void motor_set_position(int xpos, int ypos, int stepspeed) 135 | { 136 | struct motor_message msg; 137 | motor_status_get(&msg); 138 | 139 | int deltax = xpos - msg.x; 140 | int deltay = ypos - msg.y; 141 | 142 | printf(" -> set position current X: %d, Y: %d, steps required X: %d, Y: %d, speed %d\n", msg.x, msg.y, deltax, deltay, stepspeed); 143 | motor_steps(deltax, deltay, stepspeed); 144 | 145 | motor_wait_idle(); 146 | } 147 | 148 | void show_status() 149 | { 150 | unsigned int maxx, maxy; 151 | struct motor_message steps; 152 | 153 | motor_get_maxsteps(&maxx, &maxy); 154 | printf("Max X Steps %d.\n", maxx); 155 | printf("Max Y Steps %d.\n", maxy); 156 | 157 | motor_status_get(&steps); 158 | printf("Status Move: %d.\n", steps.status); 159 | printf("X Steps %d.\n", steps.x); 160 | printf("Y Steps %d.\n", steps.y); 161 | printf("Speed %d.\n", steps.speed); 162 | } 163 | 164 | void JSON_status() 165 | { 166 | // return xpos,ypos and status in JSON string 167 | // allows passing straight back to async call from ptzclient.cgi 168 | // with little effort and ability to track x,y position 169 | struct motor_message steps; 170 | 171 | motor_status_get(&steps); 172 | printf("{"); 173 | printf("\"status\":\"%d\"", steps.status); 174 | printf(","); 175 | printf("\"xpos\":\"%d\"", steps.x); 176 | printf(","); 177 | printf("\"ypos\":\"%d\"", steps.y); 178 | printf(","); 179 | printf("\"speed\":\"%d\"", steps.speed); 180 | printf("}"); 181 | } 182 | 183 | void JSON_initial() 184 | { 185 | // return all known parameters in JSON string 186 | // idea is when client page loads in browser we 187 | // get current details from camera 188 | struct motor_message steps; 189 | unsigned int maxx, maxy; 190 | 191 | motor_status_get(&steps); 192 | printf("{"); 193 | printf("\"status\":\"%d\"", steps.status); 194 | printf(","); 195 | printf("\"xpos\":\"%d\"", steps.x); 196 | printf(","); 197 | printf("\"ypos\":\"%d\"", steps.y); 198 | 199 | motor_get_maxsteps(&maxx, &maxy); 200 | printf(","); 201 | printf("\"xmax\":\"%d\"", maxx); 202 | printf(","); 203 | printf("\"ymax\":\"%d\"", maxy); 204 | printf(","); 205 | printf("\"speed\":\"%d\"", steps.speed); 206 | 207 | printf("}"); 208 | } 209 | 210 | int main(int argc, char *argv[]) 211 | { 212 | char direction = 's'; 213 | int stepspeed = 900; 214 | int xpos = 0; 215 | int ypos = 0; 216 | int got_x = 0; 217 | int got_y = 0; 218 | int c; 219 | struct motor_message pos; 220 | 221 | fd = open("/dev/motor", 0); // T31 sources don't take into account the open mode 222 | 223 | while ((c = getopt(argc, argv, "d:s:x:y:jiSr")) != -1) 224 | { 225 | switch (c) 226 | { 227 | case 'd': 228 | direction = optarg[0]; 229 | break; 230 | case 's': 231 | if (atoi(optarg) > 900) 232 | { 233 | stepspeed = 900; 234 | } 235 | else 236 | { 237 | stepspeed = atoi(optarg); 238 | } 239 | 240 | break; 241 | case 'x': 242 | xpos = atoi(optarg); 243 | got_x = 1; 244 | break; 245 | case 'y': 246 | ypos = atoi(optarg); 247 | got_y = 1; 248 | break; 249 | case 'j': 250 | // get x and y current positions and status 251 | JSON_status(); 252 | exit(EXIT_SUCCESS); 253 | break; 254 | case 'i': 255 | // get all initial values 256 | JSON_initial(); 257 | exit(EXIT_SUCCESS); 258 | break; 259 | case 'r': // reset 260 | printf(" == Reset position, please wait\n"); 261 | struct motor_reset_data motor_reset_data; 262 | memset(&motor_reset_data, 0, sizeof(motor_reset_data)); 263 | ioctl(fd, MOTOR_RESET, &motor_reset_data); 264 | break; 265 | case 'S': // status 266 | show_status(); 267 | break; 268 | default: 269 | printf("Invalid Argument %c\n", c); 270 | printf("Usage : %s\n" 271 | "\t -d Direction step\n" 272 | "\t -s Speed step (default 900)\n" 273 | "\t -x X position/step (default 0)\n" 274 | "\t -y Y position/step (default 0) .\n" 275 | "\t -r reset to default pos.\n" 276 | "\t -j return json string xpos,ypos,status.\n" 277 | "\t -i return json string for all camera parameters\n" 278 | "\t -S show status\n", 279 | argv[0]); 280 | exit(EXIT_FAILURE); 281 | } 282 | } 283 | 284 | switch (direction) 285 | { 286 | case 's': // stop 287 | motor_ioctl(MOTOR_STOP, NULL); 288 | break; 289 | 290 | case 'c': // cruise 291 | motor_ioctl(MOTOR_CRUISE, NULL); 292 | motor_wait_idle(); 293 | break; 294 | 295 | case 'b': // go back 296 | motor_status_get(&pos); 297 | printf("Going from X %d, Y %d...\n", pos.x, pos.y); 298 | motor_ioctl(MOTOR_GOBACK, NULL); 299 | motor_wait_idle(); 300 | motor_status_get(&pos); 301 | printf("To X %d, Y %d...\n", pos.x, pos.y); 302 | 303 | break; 304 | 305 | case 'h': // set position 306 | motor_status_get(&pos); 307 | if (got_x == 0) 308 | xpos = pos.x; 309 | if (got_y == 0) 310 | ypos = pos.y; 311 | motor_set_position(xpos, ypos, stepspeed); 312 | break; 313 | 314 | case 'g': // x y steps 315 | motor_steps(xpos, ypos, stepspeed); 316 | break; 317 | 318 | default: 319 | printf("Invalid Direction Argument %c\n", direction); 320 | printf("Usage : %s -d\n" 321 | "\t s (Stop)\n" 322 | "\t c (Cruise)\n" 323 | "\t b (Go to previous position)\n" 324 | "\t h (Set position X and Y)\n" 325 | "\t g (Steps X and Y)\n", 326 | argv[0]); 327 | exit(EXIT_FAILURE); 328 | } 329 | return 0; 330 | } 331 | -------------------------------------------------------------------------------- /xm-kmotor/Makefile: -------------------------------------------------------------------------------- 1 | all: xm-kmotor-orig xm-kmotor-openipc 2 | 3 | xm-kmotor-openipc: CC=arm-openipc-linux-musleabi-gcc 4 | 5 | xm-kmotor-orig: CC=arm-xm-linux-gcc 6 | 7 | xm-%: main.o 8 | $(CC) -o $@ $^ 9 | 10 | clean: 11 | -rm -rf xm-kmotor-* *.o 12 | -------------------------------------------------------------------------------- /xm-kmotor/Readme.md: -------------------------------------------------------------------------------- 1 | 2 | ## Load kernel module before use 3 | xm-kmotor is a command line tool to be able to send commands to the kmotor.ko camera module. By default this module is not loaded and so it is necessary to enter the following command. 4 | 5 | ``` 6 | insmod kmotor.ko gpio_pin=3,70,75,77,74,76,69,71,-1,-1,-1,-1 auto_test=0 7 | ``` 8 | The auto_test option can be set to 0 or 1 where auto_test=1 will get the camera to do a startup test and rotate along both the x and y axis to get the min/max x and y parameters. Setting auto_test=0 will not. 9 | 10 | After the autotest has completed be aware the camera will NOT return to its original position. 11 | 12 | ----- 13 | 14 | ### ХМ 00030695, xm-pt817-20w (tested) 15 | 16 | ``` 17 | insmod kmotor.ko gpio_pin=3,70,75,77,74,76,69,71,-1,-1,-1,-1 auto_test=1 MAX_DEGREE_X=350 MAX_DEGREE_Y=125 18 | ``` 19 | 20 | ----- 21 | 22 | ### Examples for xm-pt817-20w 23 | 24 | 1) connect to the camera via ssh 25 | ``` 26 | ssh root@ip.of.your.camera 27 | ``` 28 | 2) load the kernel module: 29 | 30 | check if the kmotor module is loaded: 31 | ``` 32 | lsmod 33 | ``` 34 | if kmotor is in the list then unload it first: 35 | ``` 36 | rmmod kmotor 37 | ``` 38 | load the kmotor module with parameters ( you may need to experiment with the MAX X and MAX Y values for your specific camera): 39 | 40 | ``` 41 | insmod /lib/modules/3.10.103\+/xiongmai/kmotor.ko gpio_pin=3,70,75,77,74,76,69,71,-1,-1,-1,-1 auto_test=1 MAX_DEGREE_X=350 MAX_DEGREE_Y=125 42 | ``` 43 | 44 | 3) testing 45 | 46 | By entering no command line arguments the current status and x,y parameters will be returned: 47 | ``` 48 | xm-kmotor 49 | ``` 50 | it will look like this: 51 | ``` 52 | Max X Steps 3982. (value of X cordinate for camera from 0 to 3982) 53 | Max Y Steps 1422. (value of Y cordinate for camera from 0 to 1422) 54 | Status Move: 65535. (65535 means idle) 55 | X Steps 1991. (current x position ) 56 | Y Steps 0. (current Y position) 57 | ``` 58 | 59 | ### Command line options 60 | ``` 61 | Usage : xm-kmotor -d Direction to step 62 | u (Up) 63 | d (Down) 64 | l (Left) 65 | r (Right) 66 | e (Right Up) 67 | c (Right Down) 68 | q (Left Up) 69 | z (Left Down) 70 | s (Stop) 71 | h (Set position X and Y) 72 | t (Go to X and Y) 73 | f (Scan, Y to set) 74 | g (Steps X and Y) 75 | -s Speed step (1-10, default 5) 76 | -x X position/step (default 0) 77 | -y Y position/step (default 0) 78 | 79 | -j get camera status as a json formatted string (currentX,currentY & status) 80 | -i get initial camera details as json string (maxX, maxY, status, currentX, currentY) 81 | ``` 82 | 83 | examples: 84 | 85 | go to mid position of X and Y: 86 | ``` 87 | xm-kmotor -d t -x 1992 -y 712 ("direction go to x=1992 y=712") 88 | ``` 89 | go to position of begining of X and Y 90 | ``` 91 | xm-kmotor -d t -x 0 -y 0 92 | ``` 93 | go to x 1992 and y 0 94 | ``` 95 | xm-kmotor -d t -x 1992 -y 0 96 | ``` 97 | get camera details as json string 98 | ``` 99 | xm-kmotor -i 100 | ``` 101 | scan up & stop 102 | ``` 103 | xm-kmotor -d u 104 | xm-kmotor -d s 105 | ``` 106 | 107 | -------------------------------------------------------------------------------- /xm-kmotor/main.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #define MOTOR_STATUS 0x80184D02 13 | #define MOTOR_MAXSTEPS 0x80184D03 14 | #define MOTOR_CMD 0x40184D01u 15 | 16 | /* 17 | s[0] = 1; //left 18 | s[0] = 2; //right 19 | s[0] = 4; //up 20 | s[0] = 8; //down 21 | s[0] = 18; //stop 22 | s[0] = 6; //rightup 23 | s[0] = 10; //rightdown 24 | s[0] = 5; //leftup 25 | s[0] = 9; //leftdown 26 | s[0] = 17; //goto position 27 | s[0] = 20; //set position 28 | s[0] = 16; //x scan 29 | s[0] = 19; //x y steps 30 | */ 31 | 32 | void motor_ioctl(int cmd, int *arg) { 33 | int fd = open("/dev/motor", O_WRONLY); 34 | ioctl(fd, cmd, arg); 35 | close(fd); 36 | } 37 | 38 | void motor_status_get(int *cmd) { motor_ioctl(MOTOR_STATUS, cmd); } 39 | 40 | void motor_get_maxsteps(int *cmd) { motor_ioctl(MOTOR_MAXSTEPS, cmd); } 41 | 42 | void motor_steps(int xpos, int ypos, int sspeed) { 43 | int s[5]; 44 | s[0] = 19; 45 | s[1] = sspeed; // xspeed 46 | s[2] = sspeed; // yspeed 47 | s[3] = xpos; // steps_x 48 | s[4] = ypos; // steps_y 49 | motor_ioctl(MOTOR_CMD, s); 50 | } 51 | 52 | void motor_scan(int sspeed, int ypos) { 53 | int s[5]; 54 | int maxsteps[2]; 55 | motor_get_maxsteps(maxsteps); 56 | s[0] = 16; 57 | s[1] = sspeed; // xspeed 58 | s[2] = 0; // left_limit_x 59 | s[3] = ypos; // maxsteps[2]; //200; //y_position 60 | s[4] = maxsteps[1]; // 3300; //right_limit_x 61 | motor_ioctl(MOTOR_CMD, s); 62 | } 63 | 64 | void motor_xy_position(int xpos, int ypos, int sspeed) { 65 | int s[5]; 66 | s[0] = 17; 67 | s[1] = xpos; 68 | s[2] = ypos; 69 | s[3] = sspeed; // xspeed 70 | s[4] = sspeed; // yspeed 71 | motor_ioctl(MOTOR_CMD, s); 72 | } 73 | 74 | void motor_set_position(int xpos, int ypos) { 75 | int s[3]; 76 | s[0] = 20; 77 | s[1] = xpos; 78 | s[2] = ypos; 79 | motor_ioctl(MOTOR_CMD, s); 80 | } 81 | 82 | void show_sage() { 83 | int steps[6]; 84 | int maxsteps[3]; 85 | 86 | motor_get_maxsteps(maxsteps); 87 | printf("Max X Steps %d.\n", maxsteps[1]); 88 | printf("Max Y Steps %d.\n", maxsteps[2]); 89 | 90 | motor_status_get(steps); 91 | printf("Status Move: %d.\n", steps[0]); 92 | printf("X Steps %d.\n", steps[1]); 93 | printf("Y Steps %d.\n", steps[2]); 94 | printf("Unknow: %d.\n", steps[3]); 95 | printf("Unknow: %d.\n", steps[4]); 96 | printf("Unknow: %d.\n", steps[5]); 97 | } 98 | 99 | void JSON_status(){ 100 | //return xpos,ypos and status in JSON string 101 | //allows passing straight back to async call from ptzclient.cgi 102 | //with little effort and ability to track x,y position 103 | int steps[6]; 104 | 105 | motor_status_get(steps); 106 | printf("{"); 107 | printf("\"status\":\"%d\"", steps[0]); 108 | printf(","); 109 | printf("\"xpos\":\"%d\"", steps[1]); 110 | printf(","); 111 | printf("\"ypos\":\"%d\"", steps[2]); 112 | printf(","); 113 | printf("\"unknown\":\"%d\"", steps[3]); 114 | printf(","); 115 | printf("\"unknown\":\"%d\"", steps[4]); 116 | printf(","); 117 | printf("\"unknown\":\"%d\"", steps[5]); 118 | printf("}"); 119 | } 120 | 121 | void JSON_initial(){ 122 | //return all known parameters in JSON string 123 | //idea is when client page loads in browser we 124 | //get current details from camera 125 | int steps[6]; 126 | int maxsteps[3]; 127 | 128 | motor_status_get(steps); 129 | printf("{"); 130 | printf("\"status\":\"%d\"", steps[0]); 131 | printf(","); 132 | printf("\"xpos\":\"%d\"", steps[1]); 133 | printf(","); 134 | printf("\"ypos\":\"%d\"", steps[2]); 135 | 136 | motor_get_maxsteps(maxsteps); 137 | printf(","); 138 | printf("\"xmax\":\"%d\"", maxsteps[1]); 139 | printf(","); 140 | printf("\"ymax\":\"%d\"", maxsteps[2]); 141 | printf(","); 142 | printf("\"unknown\":\"%d\"", steps[3]); 143 | printf(","); 144 | printf("\"unknown\":\"%d\"", steps[4]); 145 | printf(","); 146 | printf("\"unknown\":\"%d\"", steps[5]); 147 | 148 | printf("}"); 149 | 150 | } 151 | 152 | 153 | void sendCommand(int cmd, int sspeed) { 154 | int s[3]; 155 | s[0] = cmd; 156 | s[1] = sspeed; // xspeed 157 | s[2] = sspeed; // yspeed 158 | motor_ioctl(MOTOR_CMD, s); 159 | 160 | show_sage(); 161 | } 162 | 163 | int main(int argc, char *argv[]) { 164 | char direction = 's'; 165 | int stepspeed = 5; 166 | int xpos = 0; 167 | int ypos = 0; 168 | int c; 169 | 170 | while ((c = getopt(argc, argv, "d:s:x:y:ji")) != -1) { 171 | switch (c) { 172 | case 'd': 173 | direction = optarg[0]; 174 | break; 175 | case 's': 176 | if (atoi(optarg) > 10) { 177 | stepspeed = 10; 178 | } else { 179 | stepspeed = atoi(optarg); 180 | } 181 | 182 | break; 183 | case 'x': 184 | xpos = atoi(optarg); 185 | break; 186 | case 'y': 187 | ypos = atoi(optarg); 188 | break; 189 | case 'j': 190 | //get x and y current positions and status 191 | JSON_status(); 192 | exit (EXIT_SUCCESS); 193 | break; 194 | case 'i': 195 | //get all initial values 196 | JSON_initial(); 197 | exit (EXIT_SUCCESS); 198 | break; 199 | default: 200 | printf("Invalid Argument %c\n", c); 201 | printf("Usage : %s\n" 202 | "\t -d Direction step\n" 203 | "\t -s Speed step (default 5)\n" 204 | "\t -x X position/step (default 0)\n" 205 | "\t -y Y position/step (default 0) .\n" 206 | "\t -j return json string xpos,ypos,status.\n" 207 | "\t -i return json string for all camera parameters\n", 208 | argv[0]); 209 | exit(EXIT_FAILURE); 210 | } 211 | } 212 | 213 | switch (direction) { 214 | case 'u': // up 215 | sendCommand(4, stepspeed); 216 | break; 217 | 218 | case 'd': // down 219 | sendCommand(8, stepspeed); 220 | break; 221 | 222 | case 'l': // left 223 | sendCommand(1, stepspeed); 224 | break; 225 | 226 | case 'r': // right 227 | sendCommand(2, stepspeed); 228 | break; 229 | 230 | case 'e': // rightup 231 | sendCommand(6, stepspeed); 232 | break; 233 | 234 | case 'c': // rightdown 235 | sendCommand(10, stepspeed); 236 | break; 237 | 238 | case 'q': // leftup 239 | sendCommand(5, stepspeed); 240 | break; 241 | 242 | case 'z': // leftdown 243 | sendCommand(9, stepspeed); 244 | break; 245 | 246 | case 's': // stop 247 | sendCommand(18, stepspeed); 248 | break; 249 | case 'h': // set position 250 | motor_set_position(xpos, ypos); 251 | break; 252 | 253 | case 't': // xy position 254 | motor_xy_position(xpos, ypos, stepspeed); 255 | break; 256 | 257 | case 'f': // x scan 258 | motor_scan(stepspeed, ypos); 259 | break; 260 | 261 | case 'g': // x y steps 262 | motor_steps(xpos, ypos, stepspeed); 263 | break; 264 | 265 | default: 266 | printf("Invalid Direction Argument %c\n", direction); 267 | printf("Usage : %s -d\n" 268 | "\t u (Up)\n" 269 | "\t d (Down)\n" 270 | "\t l (Left)\n" 271 | "\t r (Right)\n" 272 | "\t e (Right Up)\n" 273 | "\t c (Right Down)\n" 274 | "\t q (Left Up)\n" 275 | "\t z (Left Down)\n" 276 | "\t s (Stop)\n" 277 | "\t h (Set position X and Y)\n" 278 | "\t t (Go to X and Y)\n" 279 | "\t f (Scan, Y to set)\n" 280 | "\t g (Steps X and Y)\n", 281 | argv[0]); 282 | exit(EXIT_FAILURE); 283 | } 284 | return 0; 285 | } 286 | -------------------------------------------------------------------------------- /xm-uart/Makefile: -------------------------------------------------------------------------------- 1 | all: xm-uart-motors-orig xm-uart-motors-openipc 2 | 3 | xm-uart-motors-openipc: CC=arm-openipc-linux-musleabi-gcc 4 | 5 | xm-uart-motors-orig: CC=arm-hisiv510-linux-gcc 6 | 7 | xm-%: main.o 8 | $(CC) -o $@ $^ 9 | 10 | clean: 11 | -rm -rf xm-uart-motors-* *.o 12 | -------------------------------------------------------------------------------- /xm-uart/Readme.md: -------------------------------------------------------------------------------- 1 | ## xm-uart 2 | 3 | A simple but well working implementation of the Pelco-D protocol 4 | -------------------------------------------------------------------------------- /xm-uart/main.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | const int addressPTZ = 1; 15 | 16 | #define AUTO_FOCUS 17 | 18 | #if defined(AUTO_FOCUS) 19 | #define SYNC 0xc5 20 | #else 21 | #define SYNC 0xff 22 | #endif 23 | 24 | const uint8_t init[] = {0xa5, 0x7b, 0x9e, 0xf0, 0xef, 0xee, 0xe0, 0xf4}; 25 | 26 | static void send_cmd(int fd, int panSpeed, int tiltSpeed, int zoomSpeed) { 27 | uint8_t command1 = 0, command2 = 0, data1 = 0, data2 = 0, checkSum = 0; 28 | 29 | if (panSpeed < 0) { 30 | command2 |= 0x04; 31 | panSpeed *= (-1); 32 | } else if (panSpeed > 0) { 33 | command2 |= 0x02; 34 | } 35 | data1 = (uint8_t)panSpeed * 63 / 100; 36 | 37 | if (tiltSpeed < 0) { 38 | command2 |= 0x10; 39 | tiltSpeed *= (-1); 40 | } else if (tiltSpeed > 0) { 41 | command2 |= 0x08; 42 | } 43 | data2 = (uint8_t)tiltSpeed * 63 / 100; 44 | 45 | if (zoomSpeed < 0) { 46 | command2 |= 0x40; 47 | } else if (zoomSpeed > 0) { 48 | command2 |= 0x20; 49 | } 50 | 51 | checkSum = (addressPTZ + command1 + command2 + data1 + data2) % 100; 52 | 53 | uint8_t bstr[] = {SYNC, addressPTZ, command1, command2, 54 | data1, data2, checkSum, 0x5c}; 55 | write(fd, bstr, sizeof(bstr)); 56 | } 57 | 58 | static void set_focus(int fd, bool near) { 59 | uint8_t command1 = 0, command2 = 0, data1 = 0, data2 = 0, checkSum = 0; 60 | checkSum = (addressPTZ + command1 + command2 + data1 + data2) % 100; 61 | 62 | if (near) 63 | command1 = 1; 64 | else 65 | command2 = 0x80; 66 | 67 | uint8_t bstr[] = {SYNC, addressPTZ, command1, command2, 68 | data1, data2, checkSum, 0x5c}; 69 | write(fd, bstr, sizeof(bstr)); 70 | } 71 | 72 | static void init_ptz(int fd) { write(fd, init, 8); } 73 | 74 | #define CMD(name, panSpeed, tiltSpeed, zoomSpeed) \ 75 | puts(name); \ 76 | send_cmd(uart, panSpeed, tiltSpeed, zoomSpeed); \ 77 | break; 78 | 79 | static void dump_hex(const void *data, size_t size) { 80 | char ascii[17]; 81 | size_t i, j; 82 | ascii[16] = '\0'; 83 | for (i = 0; i < size; ++i) { 84 | printf("%02X ", ((uint8_t *)data)[i]); 85 | if (((uint8_t *)data)[i] >= ' ' && ((uint8_t *)data)[i] <= '~') { 86 | ascii[i % 16] = ((uint8_t *)data)[i]; 87 | } else { 88 | ascii[i % 16] = '.'; 89 | } 90 | if ((i + 1) % 8 == 0 || i + 1 == size) { 91 | printf(" "); 92 | if ((i + 1) % 16 == 0) { 93 | printf("| %s \n", ascii); 94 | } else if (i + 1 == size) { 95 | ascii[(i + 1) % 16] = '\0'; 96 | if ((i + 1) % 16 <= 8) { 97 | printf(" "); 98 | } 99 | for (j = (i + 1) % 16; j < 16; ++j) { 100 | printf(" "); 101 | } 102 | printf("| %s \n", ascii); 103 | } 104 | } 105 | } 106 | } 107 | 108 | static void parse_incoming(const char *data, size_t size) { 109 | if ((size > 0) && (data[0] == 'X')) { 110 | char buf[20] = {0}; 111 | memcpy(buf, data, MIN(sizeof(buf) - 1, size)); 112 | printf("Magnification: %s\n", buf); 113 | return; 114 | } 115 | 116 | if ((size == 5) && !memcmp(data, "\xEF\x01\x02\x01", 4)) { 117 | if (data[4] == 1) { 118 | printf("NIGHT mode\n"); 119 | return; 120 | } else if (data[4] == 0) { 121 | printf("DAY mode\n"); 122 | return; 123 | } 124 | } 125 | 126 | dump_hex(data, size); 127 | } 128 | 129 | int main(int argc, char *argv[]) { 130 | struct termios ctrl; 131 | tcgetattr(STDIN_FILENO, &ctrl); 132 | ctrl.c_lflag &= ~ICANON; // turning off canonical mode makes input unbuffered 133 | ctrl.c_lflag &= ~ECHO; // disable echo 134 | tcsetattr(STDIN_FILENO, TCSANOW, &ctrl); 135 | int c; 136 | char *device = "/dev/ttyAMA0"; 137 | 138 | while ((c = getopt(argc, argv, "d:")) != -1) { 139 | switch (c) { 140 | case 'd': 141 | device = optarg; 142 | break; 143 | default: 144 | printf("Invalid Argument %c\n", c); 145 | printf("Usage : %s\n"); 146 | printf("\t -d tty device, default /dev/ttyAMA0\n\n"); 147 | printf("Commands:\n+ - (Zoom) z x (Focus) h j k l (Pan Tilt) Space (Cancel)\n"); 148 | return (-1); 149 | } 150 | } 151 | 152 | int uart = open(device, O_RDWR | O_NOCTTY); 153 | if (uart == -1) { 154 | printf("Error no is : %d\n", errno); 155 | printf("Error description is : %s\n", strerror(errno)); 156 | return (-1); 157 | }; 158 | 159 | struct termios options; 160 | tcgetattr(uart, &options); 161 | cfsetspeed(&options, B115200); 162 | 163 | options.c_cflag &= ~CSIZE; // Mask the character size bits 164 | options.c_cflag |= CS8; // 8 bit data 165 | options.c_cflag &= ~PARENB; // set parity to no 166 | options.c_cflag &= ~PARODD; // set parity to no 167 | options.c_cflag &= ~CSTOPB; // set one stop bit 168 | 169 | options.c_cflag |= (CLOCAL | CREAD); 170 | 171 | options.c_oflag &= ~OPOST; 172 | 173 | options.c_lflag &= 0; 174 | options.c_iflag &= 0; // disable software flow controll 175 | options.c_oflag &= 0; 176 | 177 | cfmakeraw(&options); 178 | tcsetattr(uart, TCSANOW, &options); 179 | 180 | init_ptz(uart); 181 | int flags = fcntl(uart, F_GETFL, 0); 182 | fcntl(uart, F_SETFL, flags | O_NONBLOCK); 183 | 184 | printf("Xiongmai UART Motors, get in a car and fasten your safety belt\n"); 185 | printf( 186 | "Commands:\n+ - (Zoom) z x (Focus) h j k l (Pan Tilt) Space (Cancel)\n"); 187 | 188 | while (1) { 189 | 190 | struct pollfd pfds[2] = { 191 | {.fd = 0, .events = POLLIN}, 192 | {.fd = uart, .events = POLLIN}, 193 | }; 194 | 195 | poll(pfds, 2, -1); 196 | 197 | if (pfds[0].revents & POLLIN) { 198 | char ch; 199 | int i = read(0, &ch, 1); 200 | 201 | if (!i) { 202 | 203 | printf("stdin closed\n"); 204 | return 0; 205 | } 206 | switch (ch) { 207 | case '-': 208 | CMD("Zoom out", 0, 0, -1); 209 | 210 | case '+': 211 | CMD("Zoom in", 0, 0, 1); 212 | 213 | case '\n': 214 | case ' ': 215 | CMD("Cancel", 0, 0, 0); 216 | 217 | case 'z': 218 | puts("Focus near"); 219 | set_focus(uart, true); 220 | break; 221 | 222 | case 'x': 223 | puts("Focus far"); 224 | set_focus(uart, false); 225 | break; 226 | 227 | // top 228 | case 'h': 229 | CMD("Pan left", 100, 0, 0); 230 | 231 | case 'i': 232 | case 'l': 233 | CMD("Pan right", -100, 0, 0); 234 | 235 | case 'n': 236 | case 'j': 237 | CMD("Tilt down", 0, -100, 0); 238 | 239 | case 'e': 240 | case 'k': 241 | CMD("Tilt up", 0, 100, 0); 242 | 243 | default: 244 | printf("Unknown command %c\n", ch); 245 | } 246 | } 247 | 248 | if (pfds[1].revents & POLLIN) { 249 | uint8_t rbuf[1024]; 250 | 251 | int i = read(uart, rbuf, sizeof(rbuf)); 252 | 253 | if (!i) { 254 | printf("UART closed, make sure system getty is disabled on UART\n"); 255 | return 0; 256 | } 257 | 258 | if (i != -1) 259 | parse_incoming(rbuf, i); 260 | } 261 | } 262 | send_cmd(uart, 0, 0, 0); 263 | close(uart); 264 | } 265 | --------------------------------------------------------------------------------