Add robot gimbal position push topic

include/message.h

@@ -312,6 +312,16 @@ struct PACKED PushPeriodMsg {
             float y;
             float z;
         } position;
+        struct {
+            uint16_t yaw_ground;
+            uint16_t pitch_ground;
+            uint16_t yaw;
+            uint16_t pitch;
+            struct {
+                uint8_t option_mode : 2;
+                uint8_t return_center : 1;
+            };
+        } gimbal;
     };
     struct Footer footer;
 };

src/robo.c

@@ -30,6 +30,11 @@ struct RobotImp {
 
     float x, y, z;
 
+    uint16_t yaw_ground;
+    uint16_t pitch_ground;
+    uint16_t yaw;
+    uint16_t pitch;
+
     enum {
         CONNECTING_TO_PROXY_PORT,
         SETTING_SDK_CONNECTION,
@@ -39,6 +44,7 @@ struct RobotImp {
         SUSBCRIBING_SUBNODE,
         SETTING_MOVEMENT_MODE,
         SUBSCRIBING_POSITION_STATUS,
+        SUBSCRIBING_GIMBAL_STATUS,
         SENDING_HEARTBEAT,
         WAITING,
         READY,
@@ -177,6 +183,14 @@ robot_work(Robot robot) {
         {
             add_sub_msg ( &req, robot->seq++, true, 20, 1, DDS_POSITION );
             req_send(robot->client->dev_conn, &req);
+            robot->state = SUBSCRIBING_GIMBAL_STATUS;
+            break;
+        }
+
+        case SUBSCRIBING_GIMBAL_STATUS:
+        {
+            add_sub_msg ( &req, robot->seq++, true, 21, 1, DDS_GIMBAL_POS );
+            req_send(robot->client->dev_conn, &req);
             robot->state = WAITING;
             break;
         }
@@ -309,10 +323,18 @@ robot_work(Robot robot) {
             robot->state = READY;
             break;
         case PUSH_PERIOD_MSG_CMD:
-            // assume it is a position status for now
-            robot->x = resp.resp.push.position.x;
-            robot->y = resp.resp.push.position.y;
-            robot->z = resp.resp.push.position.z;
+            switch(resp.resp.push.msg_id) {
+                case 20:
+                    robot->x = resp.resp.push.position.x;
+                    robot->y = resp.resp.push.position.y;
+                    robot->z = resp.resp.push.position.z;
+                    break;
+                case 21:
+                    robot->yaw_ground = resp.resp.push.gimbal.yaw_ground;
+                    robot->pitch_ground = resp.resp.push.gimbal.pitch_ground;
+                    robot->yaw = resp.resp.push.gimbal.yaw;
+                    robot->pitch = resp.resp.push.gimbal.pitch;
+            }
             break;
     }