Add ptz functions to ipc_cmd

src/ipc_cmd/ipc_cmd/Makefile

@@ -1,13 +1,16 @@
-OBJECTS_C = ipc_cmd.o
+OBJECTS_C = ipc_cmd.o ptz.o
 OBJECTS_R = ipc_read.o
 OBJECTS_N = ipc_notify.o
-LIBS = -lpthread -lrt
+LIBS = -lpthread -lrt -lm
 
 all: ipc_cmd ipc_read ipc_notify ipc_multiplex.so
 
 ipc_cmd.o: ipc_cmd.c $(HEADERS)
 	$(CC) -c $< -D_GNU_SOURCE -std=c99 -fPIC -O2 -o $@
 
+ptz.o: ptz.c $(HEADERS)
+	$(CC) -c $< -D_GNU_SOURCE -std=c99 -fPIC -O2 -o $@
+
 ipc_read.o: ipc_read.c $(HEADERS)
 	$(CC) -c $< -D_GNU_SOURCE -std=c99 -fPIC -O2 -o $@
 

src/ipc_cmd/ipc_cmd/ipc_cmd.c

@@ -20,8 +20,10 @@
  */
 
 #include "ipc_cmd.h"
+#include "ptz.h"
 #include "getopt.h"
 #include "signal.h"
+#include "math.h"
 
 mqd_t ipc_mq;
 
@@ -56,7 +58,7 @@ void ipc_stop()
 
 void print_usage(char *progname)
 {
-    fprintf(stderr, "\nUsage: %s [t ON/OFF] [-s SENS] [-l LED] [-v WHEN] [-i IR] [-r ROTATE] [-1] [-a AIHUMANDETECTION] [-E AIVEHICLEDETECTION] [-N AIANIMALDETECTION] [-O AIMOTIONDETECTION] [-c FACEDETECTION] [-o MOTIONTRACKING] [-I MIC] [-b SOUNDDETECTION] [-B BABYCRYING] [-n SOUNDSENSITIVITY] [-m MOVE] [-p NUM] [-P] [-R NUM] [-C MODE] [-f FILE] [-S] [-T] [-d]\n\n", progname);
+    fprintf(stderr, "\nUsage: %s [t ON/OFF] [-s SENS] [-l LED] [-v WHEN] [-i IR] [-r ROTATE] [-1] [-a AIHUMANDETECTION] [-E AIVEHICLEDETECTION] [-N AIANIMALDETECTION] [-O AIMOTIONDETECTION] [-c FACEDETECTION] [-o MOTIONTRACKING] [-I MIC] [-b SOUNDDETECTION] [-B BABYCRYING] [-n SOUNDSENSITIVITY] [-m MOVE] [-g] [-j ABS_POSITION] [-J REL_POSITION] [-p NUM] [-P NAME] [-R NUM] [-C MODE] [-f FILE] [-S TIME] [-T] [-d]\n\n", progname);
     fprintf(stderr, "\t-t ON/OFF, --switch ON/OFF\n");
     fprintf(stderr, "\t\tswitch ON or OFF the cam\n");
     fprintf(stderr, "\t-s SENS, --sensitivity SENS\n");
@@ -93,10 +95,18 @@ void print_usage(char *progname)
     fprintf(stderr, "\t\tset Sound Detection Sensitivity: 30 - 90\n");
     fprintf(stderr, "\t-m MOVE, --move MOVE\n");
     fprintf(stderr, "\t\tsend PTZ command: RIGHT, LEFT, DOWN, UP or STOP\n");
+    fprintf(stderr, "\t-g, --get-ptz\n");
+    fprintf(stderr, "\t\tget PTZ position\n");
+    fprintf(stderr, "\t-j ABS_POSITION, --jump-abs ABS_POSITION\n");
+    fprintf(stderr, "\t\tmove PTZ to ABS_POSITION (x,y) in degrees (example -j 500,500)\n");
+    fprintf(stderr, "\t-J REL_POSITION, --jump-rel REL_POSITION\n");
+    fprintf(stderr, "\t\tmove PTZ to REL_POSITION (x,y) in degrees (example -J 500,500)\n");
     fprintf(stderr, "\t-p NUM, --preset NUM\n");
-    fprintf(stderr, "\t\tsend PTZ go to preset command: NUM = [0..7]\n");
-    fprintf(stderr, "\t-P, --add_preset\n");
-    fprintf(stderr, "\t\tadd PTZ preset in the first available position\n");
+    fprintf(stderr, "\t\tsend PTZ go to preset command: TOKEN = [0..7]\n");
+    fprintf(stderr, "\t-P NAME, --add_preset NAME\n");
+    fprintf(stderr, "\t\tadd PTZ preset with name NAME in the first available position\n");
+    fprintf(stderr, "\t-H, --set_home_position\n");
+    fprintf(stderr, "\t\tset home position (preset 0 for Yi cam)\n");
     fprintf(stderr, "\t-R NUM, --remove_preset NUM\n");
     fprintf(stderr, "\t\tremove PTZ preset: NUM = [0..7] or \"all\"\n");
     fprintf(stderr, "\t-C MODE, --cruise MODE\n");
@@ -137,8 +147,14 @@ int main(int argc, char ** argv)
     int babycrying = NONE;
     int soundsensitivity = NONE;
     int move = NONE;
+    int get_pos = NONE;
+    int jump_abs = NONE;
+    int jump_rel = NONE;
+    char jump_msg[24];
+    double x, y;
     int preset = NONE;
     int add_preset = NONE;
+    int set_home_position = NONE;
     int remove_preset = NONE;
     int cruise = NONE;
     int debug = 0;
@@ -176,8 +192,12 @@ int main(int argc, char ** argv)
             {"soundsensitivity",  required_argument, 0, 'n'},
             {"move",  required_argument, 0, 'm'},
             {"move-reverse",  required_argument, 0, 'M'},
+            {"get-pos", no_argument, 0, 'g'},
+            {"jump-abs", required_argument, 0, 'j'},
+            {"jump-rel", required_argument, 0, 'J'},
             {"preset",  required_argument, 0, 'p'},
-            {"add_preset",  no_argument, 0, 'P'},
+            {"add_preset",  required_argument, 0, 'P'},
+            {"set_home_position",  no_argument, 0, 'H'},
             {"remove_preset",  required_argument, 0, 'R'},
             {"cruise",  required_argument, 0, 'C'},
             {"file", required_argument, 0, 'f'},
@@ -191,7 +211,7 @@ int main(int argc, char ** argv)
         /* getopt_long stores the option index here. */
         int option_index = 0;
 
-        c = getopt_long (argc, argv, "t:s:l:v:i:r:a:E:N:O:c:o:I:b:B:n:m:M:p:PR:C:f:S:TxXdh",
+        c = getopt_long (argc, argv, "t:s:l:v:i:r:a:E:N:O:c:o:I:b:B:n:m:M:gj:J:p:P:HR:C:f:S:Txdh",
                          long_options, &option_index);
 
         /* Detect the end of the options. */
@@ -426,6 +446,42 @@ int main(int argc, char ** argv)
             }
             break;
 
+        case 'g':
+            get_pos = 1;
+            break;
+
+        case 'j':
+            jump_abs = 1;
+            if (sscanf(optarg, "%lf,%lf", &x, &y) != 2) {
+                print_usage(argv[0]);
+                exit(EXIT_FAILURE);
+            }
+            if ((x < 0.0) || (x > 360.0)) {
+                print_usage(argv[0]);
+                exit(EXIT_FAILURE);
+            }
+            if ((y < 0.0) || (y > 180.0)) {
+                print_usage(argv[0]);
+                exit(EXIT_FAILURE);
+            }
+            break;
+
+        case 'J':
+            jump_rel = 1;
+            if (sscanf(optarg, "%lf,%lf", &x, &y) != 2) {
+                print_usage(argv[0]);
+                exit(EXIT_FAILURE);
+            }
+            if ((x < -360.0) || (x > 360.0)) {
+                print_usage(argv[0]);
+                exit(EXIT_FAILURE);
+            }
+            if ((y < -180.0) || (y > 180.0)) {
+                print_usage(argv[0]);
+                exit(EXIT_FAILURE);
+            }
+            break;
+
         case 'p':
             errno = 0;    /* To distinguish success/failure after call */
             preset = strtol(optarg, &endptr, 10);
@@ -442,9 +498,14 @@ int main(int argc, char ** argv)
             break;
 
         case 'P':
+            // Ignore argument
             add_preset = 1;
             break;
 
+        case 'H':
+            set_home_position = 1;
+            break;
+
         case 'R':
             errno = 0;    /* To distinguish success/failure after call */
             if (strcasecmp("all", optarg) == 0) {
@@ -667,16 +728,104 @@ int main(int argc, char ** argv)
         mq_send(ipc_mq, IPC_SOUND_SENS_90, sizeof(IPC_SOUND_SENS_90) - 1, 0);
     }
 
-    if (move == MOVE_RIGHT) {
-        mq_send(ipc_mq, IPC_MOVE_RIGHT, sizeof(IPC_MOVE_RIGHT) - 1, 0);
-    } else if (move == MOVE_LEFT) {
-        mq_send(ipc_mq, IPC_MOVE_LEFT, sizeof(IPC_MOVE_LEFT) - 1, 0);
-    } else if (move == MOVE_DOWN) {
-        mq_send(ipc_mq, IPC_MOVE_DOWN, sizeof(IPC_MOVE_DOWN) - 1, 0);
-    } else if (move == MOVE_UP) {
-        mq_send(ipc_mq, IPC_MOVE_UP, sizeof(IPC_MOVE_UP) - 1, 0);
-    } else if (move == MOVE_STOP) {
-        mq_send(ipc_mq, IPC_MOVE_STOP, sizeof(IPC_MOVE_STOP) - 1, 0);
+    if (move != NONE) {
+        char model_suffix[16];
+        int is_ptz_value;
+
+        memset(model_suffix, '\0', sizeof(model_suffix));
+        get_model_suffix(model_suffix, sizeof(model_suffix));
+        is_ptz_value = is_ptz(model_suffix);
+
+        if (is_ptz_value == ALLWINNER_V2) {
+            move = -1 * move;
+        } else if ((is_ptz_value == ALLWINNER_V2_ALT) &&
+                ((move == MOVE_LEFT) || (move == MOVE_RIGHT))) {
+            move = -1 * move;
+        }
+
+        if (move == MOVE_RIGHT) {
+            mq_send(ipc_mq, IPC_MOVE_RIGHT, sizeof(IPC_MOVE_RIGHT) - 1, 0);
+        } else if (move == MOVE_LEFT) {
+            mq_send(ipc_mq, IPC_MOVE_LEFT, sizeof(IPC_MOVE_LEFT) - 1, 0);
+        } else if (move == MOVE_DOWN) {
+            mq_send(ipc_mq, IPC_MOVE_DOWN, sizeof(IPC_MOVE_DOWN) - 1, 0);
+        } else if (move == MOVE_UP) {
+            mq_send(ipc_mq, IPC_MOVE_UP, sizeof(IPC_MOVE_UP) - 1, 0);
+        } else if (move == MOVE_STOP) {
+            mq_send(ipc_mq, IPC_MOVE_STOP, sizeof(IPC_MOVE_STOP) - 1, 0);
+        }
+    }
+
+    if (jump_abs != NONE) {
+        double fx, fy;
+        int cur_x, cur_y;
+        int fin_x, fin_y;
+        char model_suffix[16];
+        int is_ptz_value;
+
+        memset(model_suffix, '\0', sizeof(model_suffix));
+        get_model_suffix(model_suffix, sizeof(model_suffix));
+        is_ptz_value = is_ptz(model_suffix);
+
+        if (is_ptz_value == ALLWINNER_V2_ALT) {
+            y = 180.0 - y;
+        }
+
+        if (read_ptz(&cur_x, &cur_y) != 0) {
+            fprintf(stderr, "Error reading current ptz position\n");
+            return -1;
+        }
+        fx = x * (((double) MAX_PTZ_X) / 360.0);
+        fx = round(fx);
+        fx = (fx - ((double) cur_x)) * 100.0 / 911.0 + 50.0;
+        fin_x = (int) round(fx);
+        fy = y * (((double) MAX_PTZ_Y) / 180.0);
+        fy = round(fy);
+        fy = (fy - ((double) cur_y)) * 100.0 / 694.0 + 50.0;
+        fin_y = (int) round(fy);
+
+        if (debug) fprintf(stderr, "Current x  = %d, y  = %d\n", cur_x, cur_y);
+        if (debug) fprintf(stderr, "Command xr = %d, yr = %d\n", fin_x, fin_y);
+        memcpy(jump_msg, IPC_JUMP_POSITION, sizeof(IPC_JUMP_POSITION) - 1);
+        memcpy(&jump_msg[16], &fin_x, 4);
+        memcpy(&jump_msg[20], &fin_y, 4);
+        mq_send(ipc_mq, jump_msg, sizeof(IPC_JUMP_POSITION) - 1, 0);
+    }
+
+    if (jump_rel != NONE) {
+        double fx, fy;
+        int cur_x, cur_y;
+        int fin_x, fin_y;
+        char model_suffix[16];
+        int is_ptz_value;
+
+        memset(model_suffix, '\0', sizeof(model_suffix));
+        get_model_suffix(model_suffix, sizeof(model_suffix));
+        is_ptz_value = is_ptz(model_suffix);
+
+        if (is_ptz_value == ALLWINNER_V2_ALT) {
+            y = -1.0 * y;
+        }
+
+        if (read_ptz(&cur_x, &cur_y) != 0) {
+            fprintf(stderr, "Error reading current ptz position\n");
+            return -1;
+        }
+        fx = x * (((double) MAX_PTZ_X) / 360.0);
+        fx = round(fx);
+        fx = fx * 100.0 / 911.0 + 50.0;
+        fin_x = (int) round(fx);
+        fy = y * (((double) MAX_PTZ_Y) / 180.0);
+        fy = round(fy);
+        fy = fy * 100.0 / 694.0 + 50.0;
+        fin_y = (int) round(fy);
+
+        if (debug) fprintf(stderr, "Current x  = %d, y  = %d\n", cur_x, cur_y);
+        if (debug) fprintf(stderr, "Command xr = %d, yr = %d\n", fin_x, fin_y);
+        memcpy(jump_msg, IPC_JUMP_POSITION, sizeof(IPC_JUMP_POSITION) - 1);
+        memcpy(&jump_msg[16], &fin_x, 4);
+        memcpy(&jump_msg[20], &fin_y, 4);
+        mq_send(ipc_mq, jump_msg, sizeof(IPC_JUMP_POSITION) - 1, 0);
     }
 
     if (preset != NONE) {
@@ -686,6 +835,42 @@ int main(int argc, char ** argv)
     }
 
     if (add_preset != NONE) {
+        // Ignore name because Yi doesn't support it
+        mq_send(ipc_mq, IPC_ADD_PRESET, sizeof(IPC_ADD_PRESET) - 1, 0);
+    }
+
+    if (get_pos != NONE) {
+        double fx, fy;
+        int cur_x, cur_y;
+        char model_suffix[16];
+        int is_ptz_value;
+
+        memset(model_suffix, '\0', sizeof(model_suffix));
+        get_model_suffix(model_suffix, sizeof(model_suffix));
+        is_ptz_value = is_ptz(model_suffix);
+
+        if (read_ptz(&cur_x, &cur_y) != 0) {
+            fprintf(stderr, "Error reading current ptz position\n");
+            return -1;
+        }
+        fprintf(stderr, "Current x  = %d, y  = %d\n", cur_x, cur_y);
+        fx = ((double) cur_x) / ((double) MAX_PTZ_X) * 360.0;
+        fy = ((double) cur_y) / ((double) MAX_PTZ_Y) * 180.0;
+        if (is_ptz_value == ALLWINNER_V2_ALT) {
+            fy = 180.0 - fy;
+        }
+
+        fprintf(stderr, "Degrees x  = %.1f, y  = %.1f\n", fx, fy);
+        fprintf(stdout, "%.1f,%.1f\n", fx, fy);
+    }
+
+    if (set_home_position != NONE) {
+        // Remove preset 0
+        memcpy(preset_msg, IPC_REMOVE_PRESET, sizeof(IPC_REMOVE_PRESET) - 1);
+        preset_msg[16] = 0;
+        mq_send(ipc_mq, preset_msg, sizeof(IPC_REMOVE_PRESET) - 1, 0);
+        // sleep 1 sec and add it again
+        sleep(1);
         mq_send(ipc_mq, IPC_ADD_PRESET, sizeof(IPC_ADD_PRESET) - 1, 0);
     }
 

src/ipc_cmd/ipc_cmd/ipc_cmd.h

@@ -33,6 +33,9 @@
 
 #define NONE -1
 
+#define MAX_PTZ_X 4100
+#define MAX_PTZ_Y 1250
+
 #define IPC_MOTION_START "\x01\x00\x00\x00\x02\x00\x00\x00\x7c\x00\x7c\x00\x00\x00\x00\x00"
 #define IPC_MOTION_STOP "\x01\x00\x00\x00\x02\x00\x00\x00\x7d\x00\x7d\x00\x00\x00\x00\x00"
 
@@ -80,11 +83,13 @@
 #define IPC_MOVE_UP    "\x01\x00\x00\x00\x08\x00\x00\x00\x06\x40\x06\x40\x18\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00"
 #define IPC_MOVE_STOP  "\x01\x00\x00\x00\x08\x00\x00\x00\x07\x40\x01\x00\x00\x00\x00\x00"
 
-#define MOVE_RIGHT 0
-#define MOVE_LEFT  1
-#define MOVE_DOWN  2
+#define MOVE_STOP  0
+#define MOVE_RIGHT 2
+#define MOVE_LEFT  -2
 #define MOVE_UP    3
-#define MOVE_STOP  4
+#define MOVE_DOWN  -3
+
+#define IPC_JUMP_POSITION "\x01\x00\x00\x00\x08\x00\x00\x00\x09\x40\x09\x40\x18\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"
 
 #define IPC_GOTO_PRESET    "\x01\x00\x00\x00\x08\x00\x00\x00\x02\x40\x01\x00\x04\x00\x00\x00\xff\x00\x00\x00"
 #define IPC_ADD_PRESET     "\x01\x00\x00\x00\x08\x00\x00\x00\x00\x40\x01\x00\x00\x00\x00\x00"

src/ipc_cmd/ipc_cmd/ipc_multiplex.c

@@ -19,6 +19,8 @@ ssize_t (*original_mq_receive)(mqd_t, char*, size_t, unsigned int*);
 **/
 void ipc_multiplex_initialize() {
 
+    int i;
+
     // Enable debug mode if requested
     if (getenv(ENV_IPC_MULTIPLEX_DEBUG)) {
         debug = true;
@@ -33,7 +35,7 @@ void ipc_multiplex_initialize() {
     };
 
     char queue_name[64];
-    for (int i = 1; i < 10; i++) {
+    for (i = 1; i < 10; i++) {
         sprintf(queue_name, "%s_%d", IPC_QUEUE_NAME, i);
 
         // Open the message queue or create a new one if it does not exist
@@ -64,6 +66,8 @@ inline unsigned int get_message_target(char *msg_ptr) {
 **/
 ssize_t mq_receive(mqd_t mqdes, char *msg_ptr, size_t msg_len, unsigned int *msg_prio) {
 
+    int i;
+
     // Initialize resources on first call.
     if (is_initialized == false) {
         ipc_multiplex_initialize();
@@ -74,7 +78,7 @@ ssize_t mq_receive(mqd_t mqdes, char *msg_ptr, size_t msg_len, unsigned int *msg
 
     if (debug) {
         fprintf(stderr, "*** [IPC_MULTIPLEX] ");
-        for(int i = 0; i < bytes_read; i++)
+        for(i = 0; i < bytes_read; i++)
             fprintf(stderr, "%02x ", msg_ptr[i]);
         fprintf(stderr, "\n");
     }
@@ -85,7 +89,7 @@ ssize_t mq_receive(mqd_t mqdes, char *msg_ptr, size_t msg_len, unsigned int *msg
 //    }
 
     // Resend the received message to the dispatch queues
-    for (int i = 1; i < 10; i++) {
+    for (i = 1; i < 10; i++) {
 
         // mq_send will fail with EAGAIN whenever the target message queue is full.
         if (mq_send(ipc_mq[i], msg_ptr, bytes_read, MESSAGE_PRIORITY) != 0 && errno != EAGAIN) {
@@ -95,4 +99,4 @@ ssize_t mq_receive(mqd_t mqdes, char *msg_ptr, size_t msg_len, unsigned int *msg
 
     // Return like the original function would do
     return bytes_read;
-}
+}