增加控制指令和应答的消息ID

libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp

@@ -211,13 +211,14 @@ void AC_ZR_Serial_API::get_vehicle_flight_data(Vector3l pos,Vector3l vel,Vector3
 /**
  * @brief 获取串口接收到的控制数据
  * 
+ * @param mode 飞行模式，如果不是guide则false
  * @param type 控制模式，速度模式，位置模式
  * @param data 控制的数据，根据type决定是速度还是位置
  * @param yaw_deg 偏航角
  * @return true 
  * @return false 
  */
-bool AC_ZR_Serial_API::get_control_data(ZR_Msg_Type &type,Vector3l &data,float &yaw_deg)
+bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l &data,float &yaw_deg)
 {
     static uint32_t last_time = 0;
     uint8_t crc_sum = 0;
@@ -235,22 +236,30 @@ bool AC_ZR_Serial_API::get_control_data(ZR_Msg_Type &type,Vector3l &data,float &
 
     if(crc_sum != flight_control_parser.data[CONTROL_DATA_LEN - 1]){
         Debug("crc error clc: %02x ,rece: %02x",crc_sum,flight_control_parser.msg.crc);
-        set_control_ask(flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRCRC); // 应答CRC错误
+        set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRCRC); // 应答CRC错误
         return false; // 校验失败
     }
     type = flight_control_parser.msg.type;
     if(type < ZR_Msg_Type::MSG_CONTROL_TKOFF || type > ZR_Msg_Type::MSG_CONTROL_VEL ){
-        set_control_ask(flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRTYPE); // 应答CRC错误
+        set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRTYPE); // 应答控制类型错误
         return false; // 控制类型错误
     }
-    set_control_ask(flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_OK); // 应答成功
+    if(type != ZR_Msg_Type::MSG_CONTROL_TKOFF && mode != 4){
-    // 速度或者位置，3个float
+        set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRMODE); 
-    memcpy(&data,flight_control_parser.data + 3,12);    // 第三位开始是控制数据，长度3*4
+        return false; // 飞行模式错误
+    }
+    set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_OK); // 应答成功
+    // 速度或者位置，3个int32
+    // memcpy(&data,flight_control_parser.data + 4,12);    // 第4位开始是控制数据，长度3*4
+    data.x = flight_control_parser.msg.x;
+    data.y = flight_control_parser.msg.y;
+    data.z = flight_control_parser.msg.z;
     yaw_deg = flight_control_parser.msg.yaw;
     return true;
 
 }
-void AC_ZR_Serial_API::set_control_ask(ZR_Msg_Type receive_type,ZR_Msg_ASK ask)
+
+void AC_ZR_Serial_API::set_control_ask(uint8_t msg_id,ZR_Msg_Type receive_type,ZR_Msg_ASK ask)
 {
     uint8_t crc_sum = 0;
 
@@ -258,14 +267,10 @@ void AC_ZR_Serial_API::set_control_ask(ZR_Msg_Type receive_type,ZR_Msg_ASK ask)
     flight_control_ask.msg.msg_type = ZR_Msg_Type::MSG_ASK;
     flight_control_ask.msg.length = CONTROL_ASK_LEN - 4;
 
-
+    flight_control_ask.msg.msg_id = msg_id;
     flight_control_ask.msg.receive_type = receive_type;
     flight_control_ask.msg.msg_ask = ask;
-    // crc_clc += flight_control_ask.msg.msg_head;
+
-    // crc_clc += (int)flight_control_ask.msg.msg_type;
-    // crc_clc += (int)flight_control_ask.msg.receive_type;
-    // crc_clc += (int)flight_control_ask.msg.msg_ask;
-    
     crc_sum = crc_crc8(flight_control_ask.data,CONTROL_ASK_LEN-1);
 
     flight_control_ask.msg.crc = crc_sum;

libraries/AC_ZR_APP/AC_ZR_Serial_API.h

@@ -22,9 +22,9 @@
 
 #define MSG_HEAD 0xFE
 #define FLIGHT_DATA_LEN 40
-#define CONTROL_DATA_LEN 20
+#define CONTROL_DATA_LEN 21
 #define VEHICLE_STATUS_LEN 13
-#define CONTROL_ASK_LEN 6
+#define CONTROL_ASK_LEN 7
 
 class AC_ZR_Serial_API
 {
@@ -54,6 +54,7 @@ public:
         MSG_ASK_OK = 0,	    // 成功
         MSG_ASK_ERRCRC ,    // crc校验错误
         MSG_ASK_ERRTYPE ,   // 消息类型错误
+        MSG_ASK_ERRMODE ,   // 飞行模式错误
         MSG_ASK_OUTRANGE    // 控制数据超出限制
     };
 
@@ -72,9 +73,9 @@ public:
 
     void get_vehicle_status(uint8_t mode,uint8_t in_air,uint32_t home_distance,uint16_t volt_mv,uint8_t bat_remaining);
 
-    bool get_control_data(ZR_Msg_Type &type,Vector3l &data,float &yaw_deg);
+    bool get_control_data(uint8_t mode,ZR_Msg_Type &type,Vector3l &data,float &yaw_deg);
-
+    // 接收控制指令的应答
-    void set_control_ask(ZR_Msg_Type receive_type,ZR_Msg_ASK ask);
+    void set_control_ask(uint8_t msg_id,ZR_Msg_Type receive_type,ZR_Msg_ASK ask);
 
     bool data_trans_init;
 
@@ -125,13 +126,14 @@ private:
         uint8_t head;
         ZR_Msg_Type type;
         uint8_t length;
-        // 3
+        uint8_t msg_id;
+        // 4
         int32_t x;
         int32_t y;
         int32_t z;
-        // 15
+        // 16
         float yaw;
-        // 19
+        // 10
         uint8_t crc;
     };
     union PACKED flight_control_parser_u
@@ -145,7 +147,8 @@ private:
         uint8_t msg_head;
         ZR_Msg_Type msg_type;
         uint8_t length;
-        // 3
+        uint8_t msg_id;
+        // 4
         ZR_Msg_Type receive_type;
         ZR_Msg_ASK msg_ask;
         uint8_t crc;