瀏覽代碼

增加解锁拒绝执行原因

Liu Yang 10 月之前
父節點
當前提交
9c0b27b114

+ 25 - 3
msg_definitions/VKFly.xml

@@ -111,6 +111,29 @@
       </entry>
     </enum>
 
+    <enum name="VKFLY_ARM_DENIED_REASON">
+      <description>Arm denied reason for VKFLY</description>
+      <entry value="0" name="VKFLY_ARM_DENIED_REASON_NONE"></entry>
+      <entry value="1" name="VKFLY_ARM_DENIED_NO_INS"> </entry>
+      <entry value="2" name="VKFLY_ARM_DENIED_SPD_OVER_LIM"></entry>
+      <entry value="3" name="VKFLY_ARM_DENIED_ACC_OVER_LIM"></entry>
+      <entry value="4" name="VKFLY_ARM_DENIED_GYR_OVER_LIM"></entry>
+      <entry value="5" name="VKFLY_ARM_DENIED_GPS_ERR"></entry>
+      <entry value="6" name="VKFLY_ARM_DENIED_IMU_ERR"></entry>
+      <entry value="7" name="VKFLY_ARM_DENIED_POS_NOT_FIXED"></entry>
+      <entry value="8" name="VKFLY_ARM_DENIED_RTK_NOT_FIXED"></entry>
+      <entry value="9" name="VKFLY_ARM_DENIED_MAG_ERR"></entry>
+      <entry value="10" name="VKFLY_ARM_DENIED_RESERVE"></entry>
+      <entry value="11" name="VKFLY_ARM_DENIED_TEMP_OVER_LIM"></entry>
+      <entry value="13" name="VKFLY_ARM_DENIED_OUT_FENCE"></entry>
+      <entry value="14" name="VKFLY_ARM_DENIED_LOW_BAT_VOLT"></entry>
+      <entry value="15" name="VKFLY_ARM_DENIED_LOW_BAT_CAP"></entry>
+      <entry value="16" name="VKFLY_ARM_DENIED_BAT_BMS_FAULT"></entry>
+      <entry value="17" name="VKFLY_ARM_DENIED_SERVO_FAULT"></entry>
+      <entry value="18" name="VKFLY_ARM_DENIED_LEAN_ANG"></entry>
+      <entry value="19" name="VKFLY_ARM_DENIED_IN_CALIBRATION"></entry>
+    </enum>
+
     <enum name="VKFLY_MAGCALIB_STAGE">
       <description>VKins magcalib stage</description>
       <entry value="0" name="VK_MAGCALIB_NONE">
@@ -482,8 +505,6 @@
       <entry value="1" name="VKFLY_MISSION_EXEC_MODE_NO_ACT"></entry>
       <entry value="2" name="VKFLY_MISSION_EXEC_MODE_REVERSE"></entry>
       <entry value="3" name="VKFLY_MISSION_EXEC_MODE_REVERSE_NO_ACT"></entry>
-      <entry value="4" name="VKFLY_MISSION_EXEC_MODE_REVERSE_THAN_NORMAL"></entry>
-      <entry value="5" name="VKFLY_MISSION_EXEC_MODE_REVERSE_THAN_NORMAL_NO_ACT"></entry>
     </enum>
 
     <enum name="VKFLY_MISSION_DONE_ACT">
@@ -676,7 +697,8 @@
       <field type="uint16_t" name="cell_num" minValue="0" maxValue="100">BMS cell numbers</field>
       <field type="uint16_t[30]" name="cell_volt" units="mV">BMS cell voltage in mV</field>
       <field type="uint16_t" name="cyc_cnt" minValue="0" increment="1"> charge and discharge times </field>
-      <field type="uint8_t" name="health" minValue="0" maxValue="100">battery healthiness in percentage</field>
+      <field type="uint8_t" name="health" minValue="0" maxValue="100">battery healthiness in
+        percentage</field>
     </message>
 
 

+ 29 - 4
readme.md

@@ -402,7 +402,7 @@ mavlink common 标准消息集中主要由 MAVLINK_MSG_ID_COMMAND_INT 和 MAVLIN
 
   - [MAV_CMD_COMPONENT_ARM_DISARM](https://mavlink.io/en/messages/common.html#MAV_CMD_COMPONENT_ARM_DISARM)
 
-    解锁\上锁命令
+    解锁\上锁命令, 该指令执行拒绝后飞控返回的 ack 中, result_param2 表明拒绝原因, 参考 VKFLY_ARM_DENIED_REASON
 
     | 参数   | 说明                       |
     | ------ | -------------------------- |
@@ -685,6 +685,33 @@ mavlink common 标准消息集中主要由 MAVLINK_MSG_ID_COMMAND_INT 和 MAVLIN
     VKFLY_USER_COMP_ID_ENUM_END=16, /*  | */
   }
   ```
+- **VKFLY_ARM_DENIED_REASON**
+  解锁拒绝执行原因
+  ```c
+  typedef enum VKFLY_ARM_DENIED_REASON
+  {
+    VKFLY_ARM_DENIED_REASON_NONE=0, /*  | */
+    VKFLY_ARM_DENIED_NO_INS=1, /* 航姿数据异常 | */
+    VKFLY_ARM_DENIED_SPD_OVER_LIM=2, /* 速度过大 | */
+    VKFLY_ARM_DENIED_ACC_OVER_LIM=3, /* 加速度过大 | */
+    VKFLY_ARM_DENIED_GYR_OVER_LIM=4, /* 角速度过大 | */
+    VKFLY_ARM_DENIED_GPS_ERR=5, /* GPS 异常 | */
+    VKFLY_ARM_DENIED_IMU_ERR=6, /* IMU 异常 | */
+    VKFLY_ARM_DENIED_POS_NOT_FIXED=7, /* 未定位 | */
+    VKFLY_ARM_DENIED_RTK_NOT_FIXED=8, /* RTK未锁定 | */
+    VKFLY_ARM_DENIED_MAG_ERR=9, /* 磁异常 | */
+    VKFLY_ARM_DENIED_RESERVE=10, /* 预留 | */
+    VKFLY_ARM_DENIED_TEMP_OVER_LIM=11, /* 温度过热 | */
+    VKFLY_ARM_DENIED_OUT_FENCE=13, /* 超出电子围栏范围 | */
+    VKFLY_ARM_DENIED_LOW_BAT_VOLT=14, /* 电池电压低 | */
+    VKFLY_ARM_DENIED_LOW_BAT_CAP=15, /* 电池电量低 | */
+    VKFLY_ARM_DENIED_BAT_BMS_FAULT=16, /* 电池BMS故障 | */
+    VKFLY_ARM_DENIED_SERVO_FAULT=17, /* 动力故障 | */
+    VKFLY_ARM_DENIED_LEAN_ANG=18, /* 倾斜角过大 | */
+    VKFLY_ARM_DENIED_IN_CALIBRATION=19, /* 正在校准中 | */
+    VKFLY_ARM_DENIED_REASON_ENUM_END=20, /*  | */
+  } VKFLY_ARM_DENIED_REASON;
+```
 
 - **VKFLY_FS_ACTION**
 
@@ -961,9 +988,7 @@ mavlink common 标准消息集中主要由 MAVLINK_MSG_ID_COMMAND_INT 和 MAVLIN
     VKFLY_MISSION_EXEC_MODE_NO_ACT=1, /* 顺序执行, 略过航点动作 | */
     VKFLY_MISSION_EXEC_MODE_REVERSE=2, /* 逆序执行, 正常航点动作 | */
     VKFLY_MISSION_EXEC_MODE_REVERSE_NO_ACT=3, /* 逆序执行, 忽略航点动作 | */
-    VKFLY_MISSION_EXEC_MODE_NORMAL_THAN_REVERSE=4, /* 顺序执行, 完成最后一个任务点后再逆序执行, 全程正常执行航点动作 | */
-    VKFLY_MISSION_EXEC_MODE_NORMAL_THAN_REVERSE_NO_ACT=5, /* 顺序执行, 完成最后一首歌任务点后再逆序执行, 逆序过程略过航点动作 | */
-    VKFLY_MISSION_EXEC_MODE_ENUM_END=6, /*  | */
+    VKFLY_MISSION_EXEC_MODE_ENUM_END=4, /*  | */
   } VKFLY_MISSION_EXEC_MODE;
   ```
 

+ 30 - 4
v2.0/VKFly/VKFly.h

@@ -10,7 +10,7 @@
     #error Wrong include order: MAVLINK_VKFLY.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call.
 #endif
 
-#define MAVLINK_VKFLY_XML_HASH 8768893837624279133
+#define MAVLINK_VKFLY_XML_HASH 3626662836185239078
 
 #ifdef __cplusplus
 extern "C" {
@@ -106,6 +106,34 @@ typedef enum VKFLY_ROI_STATE
 } VKFLY_ROI_STATE;
 #endif
 
+/** @brief Arm denied reason for VKFLY */
+#ifndef HAVE_ENUM_VKFLY_ARM_DENIED_REASON
+#define HAVE_ENUM_VKFLY_ARM_DENIED_REASON
+typedef enum VKFLY_ARM_DENIED_REASON
+{
+   VKFLY_ARM_DENIED_REASON_NONE=0, /*  | */
+   VKFLY_ARM_DENIED_NO_INS=1, /*  | */
+   VKFLY_ARM_DENIED_SPD_OVER_LIM=2, /*  | */
+   VKFLY_ARM_DENIED_ACC_OVER_LIM=3, /*  | */
+   VKFLY_ARM_DENIED_GYR_OVER_LIM=4, /*  | */
+   VKFLY_ARM_DENIED_GPS_ERR=5, /*  | */
+   VKFLY_ARM_DENIED_IMU_ERR=6, /*  | */
+   VKFLY_ARM_DENIED_POS_NOT_FIXED=7, /*  | */
+   VKFLY_ARM_DENIED_RTK_NOT_FIXED=8, /*  | */
+   VKFLY_ARM_DENIED_MAG_ERR=9, /*  | */
+   VKFLY_ARM_DENIED_RESERVE=10, /*  | */
+   VKFLY_ARM_DENIED_TEMP_OVER_LIM=11, /*  | */
+   VKFLY_ARM_DENIED_OUT_FENCE=13, /*  | */
+   VKFLY_ARM_DENIED_LOW_BAT_VOLT=14, /*  | */
+   VKFLY_ARM_DENIED_LOW_BAT_CAP=15, /*  | */
+   VKFLY_ARM_DENIED_BAT_BMS_FAULT=16, /*  | */
+   VKFLY_ARM_DENIED_SERVO_FAULT=17, /*  | */
+   VKFLY_ARM_DENIED_LEAN_ANG=18, /*  | */
+   VKFLY_ARM_DENIED_IN_CALIBRATION=19, /*  | */
+   VKFLY_ARM_DENIED_REASON_ENUM_END=20, /*  | */
+} VKFLY_ARM_DENIED_REASON;
+#endif
+
 /** @brief VKins magcalib stage */
 #ifndef HAVE_ENUM_VKFLY_MAGCALIB_STAGE
 #define HAVE_ENUM_VKFLY_MAGCALIB_STAGE
@@ -354,9 +382,7 @@ typedef enum VKFLY_MISSION_EXEC_MODE
    VKFLY_MISSION_EXEC_MODE_NO_ACT=1, /*  | */
    VKFLY_MISSION_EXEC_MODE_REVERSE=2, /*  | */
    VKFLY_MISSION_EXEC_MODE_REVERSE_NO_ACT=3, /*  | */
-   VKFLY_MISSION_EXEC_MODE_REVERSE_THAN_NORMAL=4, /*  | */
-   VKFLY_MISSION_EXEC_MODE_REVERSE_THAN_NORMAL_NO_ACT=5, /*  | */
-   VKFLY_MISSION_EXEC_MODE_ENUM_END=6, /*  | */
+   VKFLY_MISSION_EXEC_MODE_ENUM_END=4, /*  | */
 } VKFLY_MISSION_EXEC_MODE;
 #endif
 

+ 1 - 1
v2.0/VKFly/mavlink.h

@@ -6,7 +6,7 @@
 #ifndef MAVLINK_H
 #define MAVLINK_H
 
-#define MAVLINK_PRIMARY_XML_HASH 8768893837624279133
+#define MAVLINK_PRIMARY_XML_HASH 3626662836185239078
 
 #ifndef MAVLINK_STX
 #define MAVLINK_STX 253

+ 12 - 6
v2.0/VKFly/mavlink_msg_vk_bms_status.h

@@ -18,7 +18,8 @@ typedef struct __mavlink_vk_bms_status_t {
  int8_t cap_percent; /*< [%] BMS remaining power in percentage*/
  uint8_t bat_id; /*<  BMS id, start from
         0*/
- uint8_t health; /*<  battery healthiness in percentage*/
+ uint8_t health; /*<  battery healthiness in
+        percentage*/
 } mavlink_vk_bms_status_t;
 
 #define MAVLINK_MSG_ID_VK_BMS_STATUS_LEN 83
@@ -87,7 +88,8 @@ typedef struct __mavlink_vk_bms_status_t {
  * @param cell_num  BMS cell numbers
  * @param cell_volt [mV] BMS cell voltage in mV
  * @param cyc_cnt   charge and discharge times 
- * @param health  battery healthiness in percentage
+ * @param health  battery healthiness in
+        percentage
  * @return length of the message in bytes (excluding serial stream start sign)
  */
 static inline uint16_t mavlink_msg_vk_bms_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
@@ -147,7 +149,8 @@ static inline uint16_t mavlink_msg_vk_bms_status_pack(uint8_t system_id, uint8_t
  * @param cell_num  BMS cell numbers
  * @param cell_volt [mV] BMS cell voltage in mV
  * @param cyc_cnt   charge and discharge times 
- * @param health  battery healthiness in percentage
+ * @param health  battery healthiness in
+        percentage
  * @return length of the message in bytes (excluding serial stream start sign)
  */
 static inline uint16_t mavlink_msg_vk_bms_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg,
@@ -210,7 +213,8 @@ static inline uint16_t mavlink_msg_vk_bms_status_pack_status(uint8_t system_id,
  * @param cell_num  BMS cell numbers
  * @param cell_volt [mV] BMS cell voltage in mV
  * @param cyc_cnt   charge and discharge times 
- * @param health  battery healthiness in percentage
+ * @param health  battery healthiness in
+        percentage
  * @return length of the message in bytes (excluding serial stream start sign)
  */
 static inline uint16_t mavlink_msg_vk_bms_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
@@ -309,7 +313,8 @@ static inline uint16_t mavlink_msg_vk_bms_status_encode_status(uint8_t system_id
  * @param cell_num  BMS cell numbers
  * @param cell_volt [mV] BMS cell voltage in mV
  * @param cyc_cnt   charge and discharge times 
- * @param health  battery healthiness in percentage
+ * @param health  battery healthiness in
+        percentage
  */
 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
 
@@ -513,7 +518,8 @@ static inline uint16_t mavlink_msg_vk_bms_status_get_cyc_cnt(const mavlink_messa
 /**
  * @brief Get field health from vk_bms_status message
  *
- * @return  battery healthiness in percentage
+ * @return  battery healthiness in
+        percentage
  */
 static inline uint8_t mavlink_msg_vk_bms_status_get_health(const mavlink_message_t* msg)
 {

+ 1 - 1
v2.0/VKFly/version.h

@@ -7,7 +7,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Tue Sep 10 2024"
+#define MAVLINK_BUILD_DATE "Wed Sep 11 2024"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
  

+ 1 - 1
v2.0/common/common.h

@@ -10,7 +10,7 @@
     #error Wrong include order: MAVLINK_COMMON.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call.
 #endif
 
-#define MAVLINK_COMMON_XML_HASH 8093961037565932851
+#define MAVLINK_COMMON_XML_HASH 4784416552898012285
 
 #ifdef __cplusplus
 extern "C" {

+ 1 - 1
v2.0/common/mavlink.h

@@ -6,7 +6,7 @@
 #ifndef MAVLINK_H
 #define MAVLINK_H
 
-#define MAVLINK_PRIMARY_XML_HASH 8093961037565932851
+#define MAVLINK_PRIMARY_XML_HASH 4784416552898012285
 
 #ifndef MAVLINK_STX
 #define MAVLINK_STX 253

+ 1 - 1
v2.0/common/version.h

@@ -7,7 +7,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Tue Sep 10 2024"
+#define MAVLINK_BUILD_DATE "Wed Sep 11 2024"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
  

+ 1 - 1
v2.0/minimal/mavlink.h

@@ -6,7 +6,7 @@
 #ifndef MAVLINK_H
 #define MAVLINK_H
 
-#define MAVLINK_PRIMARY_XML_HASH -1903176616712080717
+#define MAVLINK_PRIMARY_XML_HASH 3920116293259174708
 
 #ifndef MAVLINK_STX
 #define MAVLINK_STX 253

+ 1 - 1
v2.0/minimal/minimal.h

@@ -10,7 +10,7 @@
     #error Wrong include order: MAVLINK_MINIMAL.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call.
 #endif
 
-#define MAVLINK_MINIMAL_XML_HASH -1903176616712080717
+#define MAVLINK_MINIMAL_XML_HASH 3920116293259174708
 
 #ifdef __cplusplus
 extern "C" {

+ 1 - 1
v2.0/minimal/version.h

@@ -7,7 +7,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Tue Sep 10 2024"
+#define MAVLINK_BUILD_DATE "Wed Sep 11 2024"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 22
  

+ 1 - 1
v2.0/standard/mavlink.h

@@ -6,7 +6,7 @@
 #ifndef MAVLINK_H
 #define MAVLINK_H
 
-#define MAVLINK_PRIMARY_XML_HASH 1640796470349844457
+#define MAVLINK_PRIMARY_XML_HASH 8556824078215993087
 
 #ifndef MAVLINK_STX
 #define MAVLINK_STX 253

+ 1 - 1
v2.0/standard/standard.h

@@ -10,7 +10,7 @@
     #error Wrong include order: MAVLINK_STANDARD.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call.
 #endif
 
-#define MAVLINK_STANDARD_XML_HASH 1640796470349844457
+#define MAVLINK_STANDARD_XML_HASH 8556824078215993087
 
 #ifdef __cplusplus
 extern "C" {

+ 1 - 1
v2.0/standard/version.h

@@ -7,7 +7,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Tue Sep 10 2024"
+#define MAVLINK_BUILD_DATE "Wed Sep 11 2024"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 22