|
@@ -8,6 +8,7 @@ typedef struct __mavlink_vkfmu_status_t {
|
|
|
uint32_t time_boot_ms; /*< [ms] Timestamp in ms from system boot.*/
|
|
|
uint32_t flight_time; /*< [s] flight time in seconds*/
|
|
|
uint32_t dist_to_tar; /*< [cm] distance to target position in cm*/
|
|
|
+ float flight_dist; /*< [m] flight distance since this power up*/
|
|
|
uint16_t ups_volt; /*< ups voltage in 0.1V*/
|
|
|
uint16_t adc_volt; /*< adc voltage in 0.1V*/
|
|
|
uint16_t servo_state; /*< bitmap for servo state*/
|
|
@@ -16,13 +17,13 @@ typedef struct __mavlink_vkfmu_status_t {
|
|
|
uint8_t s_flag3; /*< fmu sflag3*/
|
|
|
} mavlink_vkfmu_status_t;
|
|
|
|
|
|
-#define MAVLINK_MSG_ID_VKFMU_STATUS_LEN 21
|
|
|
-#define MAVLINK_MSG_ID_VKFMU_STATUS_MIN_LEN 21
|
|
|
-#define MAVLINK_MSG_ID_53001_LEN 21
|
|
|
-#define MAVLINK_MSG_ID_53001_MIN_LEN 21
|
|
|
+#define MAVLINK_MSG_ID_VKFMU_STATUS_LEN 25
|
|
|
+#define MAVLINK_MSG_ID_VKFMU_STATUS_MIN_LEN 25
|
|
|
+#define MAVLINK_MSG_ID_53001_LEN 25
|
|
|
+#define MAVLINK_MSG_ID_53001_MIN_LEN 25
|
|
|
|
|
|
-#define MAVLINK_MSG_ID_VKFMU_STATUS_CRC 251
|
|
|
-#define MAVLINK_MSG_ID_53001_CRC 251
|
|
|
+#define MAVLINK_MSG_ID_VKFMU_STATUS_CRC 136
|
|
|
+#define MAVLINK_MSG_ID_53001_CRC 136
|
|
|
|
|
|
|
|
|
|
|
@@ -30,31 +31,33 @@ typedef struct __mavlink_vkfmu_status_t {
|
|
|
#define MAVLINK_MESSAGE_INFO_VKFMU_STATUS { \
|
|
|
53001, \
|
|
|
"VKFMU_STATUS", \
|
|
|
- 9, \
|
|
|
+ 10, \
|
|
|
{ { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_vkfmu_status_t, time_boot_ms) }, \
|
|
|
- { "rtl_reason", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_vkfmu_status_t, rtl_reason) }, \
|
|
|
- { "loiter_reason", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_vkfmu_status_t, loiter_reason) }, \
|
|
|
- { "s_flag3", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_vkfmu_status_t, s_flag3) }, \
|
|
|
- { "ups_volt", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_vkfmu_status_t, ups_volt) }, \
|
|
|
- { "adc_volt", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_vkfmu_status_t, adc_volt) }, \
|
|
|
+ { "rtl_reason", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_vkfmu_status_t, rtl_reason) }, \
|
|
|
+ { "loiter_reason", NULL, MAVLINK_TYPE_UINT8_T, 0, 23, offsetof(mavlink_vkfmu_status_t, loiter_reason) }, \
|
|
|
+ { "s_flag3", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_vkfmu_status_t, s_flag3) }, \
|
|
|
+ { "ups_volt", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_vkfmu_status_t, ups_volt) }, \
|
|
|
+ { "adc_volt", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_vkfmu_status_t, adc_volt) }, \
|
|
|
{ "flight_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_vkfmu_status_t, flight_time) }, \
|
|
|
{ "dist_to_tar", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_vkfmu_status_t, dist_to_tar) }, \
|
|
|
- { "servo_state", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_vkfmu_status_t, servo_state) }, \
|
|
|
+ { "servo_state", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_vkfmu_status_t, servo_state) }, \
|
|
|
+ { "flight_dist", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vkfmu_status_t, flight_dist) }, \
|
|
|
} \
|
|
|
}
|
|
|
#else
|
|
|
#define MAVLINK_MESSAGE_INFO_VKFMU_STATUS { \
|
|
|
"VKFMU_STATUS", \
|
|
|
- 9, \
|
|
|
+ 10, \
|
|
|
{ { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_vkfmu_status_t, time_boot_ms) }, \
|
|
|
- { "rtl_reason", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_vkfmu_status_t, rtl_reason) }, \
|
|
|
- { "loiter_reason", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_vkfmu_status_t, loiter_reason) }, \
|
|
|
- { "s_flag3", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_vkfmu_status_t, s_flag3) }, \
|
|
|
- { "ups_volt", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_vkfmu_status_t, ups_volt) }, \
|
|
|
- { "adc_volt", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_vkfmu_status_t, adc_volt) }, \
|
|
|
+ { "rtl_reason", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_vkfmu_status_t, rtl_reason) }, \
|
|
|
+ { "loiter_reason", NULL, MAVLINK_TYPE_UINT8_T, 0, 23, offsetof(mavlink_vkfmu_status_t, loiter_reason) }, \
|
|
|
+ { "s_flag3", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_vkfmu_status_t, s_flag3) }, \
|
|
|
+ { "ups_volt", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_vkfmu_status_t, ups_volt) }, \
|
|
|
+ { "adc_volt", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_vkfmu_status_t, adc_volt) }, \
|
|
|
{ "flight_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_vkfmu_status_t, flight_time) }, \
|
|
|
{ "dist_to_tar", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_vkfmu_status_t, dist_to_tar) }, \
|
|
|
- { "servo_state", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_vkfmu_status_t, servo_state) }, \
|
|
|
+ { "servo_state", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_vkfmu_status_t, servo_state) }, \
|
|
|
+ { "flight_dist", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vkfmu_status_t, flight_dist) }, \
|
|
|
} \
|
|
|
}
|
|
|
#endif
|
|
@@ -74,22 +77,24 @@ typedef struct __mavlink_vkfmu_status_t {
|
|
|
* @param flight_time [s] flight time in seconds
|
|
|
* @param dist_to_tar [cm] distance to target position in cm
|
|
|
* @param servo_state bitmap for servo state
|
|
|
+ * @param flight_dist [m] flight distance since this power up
|
|
|
* @return length of the message in bytes (excluding serial stream start sign)
|
|
|
*/
|
|
|
static inline uint16_t mavlink_msg_vkfmu_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
|
|
|
- uint32_t time_boot_ms, uint8_t rtl_reason, uint8_t loiter_reason, uint8_t s_flag3, uint16_t ups_volt, uint16_t adc_volt, uint32_t flight_time, uint32_t dist_to_tar, uint16_t servo_state)
|
|
|
+ uint32_t time_boot_ms, uint8_t rtl_reason, uint8_t loiter_reason, uint8_t s_flag3, uint16_t ups_volt, uint16_t adc_volt, uint32_t flight_time, uint32_t dist_to_tar, uint16_t servo_state, float flight_dist)
|
|
|
{
|
|
|
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
|
|
char buf[MAVLINK_MSG_ID_VKFMU_STATUS_LEN];
|
|
|
_mav_put_uint32_t(buf, 0, time_boot_ms);
|
|
|
_mav_put_uint32_t(buf, 4, flight_time);
|
|
|
_mav_put_uint32_t(buf, 8, dist_to_tar);
|
|
|
- _mav_put_uint16_t(buf, 12, ups_volt);
|
|
|
- _mav_put_uint16_t(buf, 14, adc_volt);
|
|
|
- _mav_put_uint16_t(buf, 16, servo_state);
|
|
|
- _mav_put_uint8_t(buf, 18, rtl_reason);
|
|
|
- _mav_put_uint8_t(buf, 19, loiter_reason);
|
|
|
- _mav_put_uint8_t(buf, 20, s_flag3);
|
|
|
+ _mav_put_float(buf, 12, flight_dist);
|
|
|
+ _mav_put_uint16_t(buf, 16, ups_volt);
|
|
|
+ _mav_put_uint16_t(buf, 18, adc_volt);
|
|
|
+ _mav_put_uint16_t(buf, 20, servo_state);
|
|
|
+ _mav_put_uint8_t(buf, 22, rtl_reason);
|
|
|
+ _mav_put_uint8_t(buf, 23, loiter_reason);
|
|
|
+ _mav_put_uint8_t(buf, 24, s_flag3);
|
|
|
|
|
|
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VKFMU_STATUS_LEN);
|
|
|
#else
|
|
@@ -97,6 +102,7 @@ static inline uint16_t mavlink_msg_vkfmu_status_pack(uint8_t system_id, uint8_t
|
|
|
packet.time_boot_ms = time_boot_ms;
|
|
|
packet.flight_time = flight_time;
|
|
|
packet.dist_to_tar = dist_to_tar;
|
|
|
+ packet.flight_dist = flight_dist;
|
|
|
packet.ups_volt = ups_volt;
|
|
|
packet.adc_volt = adc_volt;
|
|
|
packet.servo_state = servo_state;
|
|
@@ -127,22 +133,24 @@ static inline uint16_t mavlink_msg_vkfmu_status_pack(uint8_t system_id, uint8_t
|
|
|
* @param flight_time [s] flight time in seconds
|
|
|
* @param dist_to_tar [cm] distance to target position in cm
|
|
|
* @param servo_state bitmap for servo state
|
|
|
+ * @param flight_dist [m] flight distance since this power up
|
|
|
* @return length of the message in bytes (excluding serial stream start sign)
|
|
|
*/
|
|
|
static inline uint16_t mavlink_msg_vkfmu_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg,
|
|
|
- uint32_t time_boot_ms, uint8_t rtl_reason, uint8_t loiter_reason, uint8_t s_flag3, uint16_t ups_volt, uint16_t adc_volt, uint32_t flight_time, uint32_t dist_to_tar, uint16_t servo_state)
|
|
|
+ uint32_t time_boot_ms, uint8_t rtl_reason, uint8_t loiter_reason, uint8_t s_flag3, uint16_t ups_volt, uint16_t adc_volt, uint32_t flight_time, uint32_t dist_to_tar, uint16_t servo_state, float flight_dist)
|
|
|
{
|
|
|
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
|
|
char buf[MAVLINK_MSG_ID_VKFMU_STATUS_LEN];
|
|
|
_mav_put_uint32_t(buf, 0, time_boot_ms);
|
|
|
_mav_put_uint32_t(buf, 4, flight_time);
|
|
|
_mav_put_uint32_t(buf, 8, dist_to_tar);
|
|
|
- _mav_put_uint16_t(buf, 12, ups_volt);
|
|
|
- _mav_put_uint16_t(buf, 14, adc_volt);
|
|
|
- _mav_put_uint16_t(buf, 16, servo_state);
|
|
|
- _mav_put_uint8_t(buf, 18, rtl_reason);
|
|
|
- _mav_put_uint8_t(buf, 19, loiter_reason);
|
|
|
- _mav_put_uint8_t(buf, 20, s_flag3);
|
|
|
+ _mav_put_float(buf, 12, flight_dist);
|
|
|
+ _mav_put_uint16_t(buf, 16, ups_volt);
|
|
|
+ _mav_put_uint16_t(buf, 18, adc_volt);
|
|
|
+ _mav_put_uint16_t(buf, 20, servo_state);
|
|
|
+ _mav_put_uint8_t(buf, 22, rtl_reason);
|
|
|
+ _mav_put_uint8_t(buf, 23, loiter_reason);
|
|
|
+ _mav_put_uint8_t(buf, 24, s_flag3);
|
|
|
|
|
|
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VKFMU_STATUS_LEN);
|
|
|
#else
|
|
@@ -150,6 +158,7 @@ static inline uint16_t mavlink_msg_vkfmu_status_pack_status(uint8_t system_id, u
|
|
|
packet.time_boot_ms = time_boot_ms;
|
|
|
packet.flight_time = flight_time;
|
|
|
packet.dist_to_tar = dist_to_tar;
|
|
|
+ packet.flight_dist = flight_dist;
|
|
|
packet.ups_volt = ups_volt;
|
|
|
packet.adc_volt = adc_volt;
|
|
|
packet.servo_state = servo_state;
|
|
@@ -183,23 +192,25 @@ static inline uint16_t mavlink_msg_vkfmu_status_pack_status(uint8_t system_id, u
|
|
|
* @param flight_time [s] flight time in seconds
|
|
|
* @param dist_to_tar [cm] distance to target position in cm
|
|
|
* @param servo_state bitmap for servo state
|
|
|
+ * @param flight_dist [m] flight distance since this power up
|
|
|
* @return length of the message in bytes (excluding serial stream start sign)
|
|
|
*/
|
|
|
static inline uint16_t mavlink_msg_vkfmu_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
|
|
|
mavlink_message_t* msg,
|
|
|
- uint32_t time_boot_ms,uint8_t rtl_reason,uint8_t loiter_reason,uint8_t s_flag3,uint16_t ups_volt,uint16_t adc_volt,uint32_t flight_time,uint32_t dist_to_tar,uint16_t servo_state)
|
|
|
+ uint32_t time_boot_ms,uint8_t rtl_reason,uint8_t loiter_reason,uint8_t s_flag3,uint16_t ups_volt,uint16_t adc_volt,uint32_t flight_time,uint32_t dist_to_tar,uint16_t servo_state,float flight_dist)
|
|
|
{
|
|
|
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
|
|
char buf[MAVLINK_MSG_ID_VKFMU_STATUS_LEN];
|
|
|
_mav_put_uint32_t(buf, 0, time_boot_ms);
|
|
|
_mav_put_uint32_t(buf, 4, flight_time);
|
|
|
_mav_put_uint32_t(buf, 8, dist_to_tar);
|
|
|
- _mav_put_uint16_t(buf, 12, ups_volt);
|
|
|
- _mav_put_uint16_t(buf, 14, adc_volt);
|
|
|
- _mav_put_uint16_t(buf, 16, servo_state);
|
|
|
- _mav_put_uint8_t(buf, 18, rtl_reason);
|
|
|
- _mav_put_uint8_t(buf, 19, loiter_reason);
|
|
|
- _mav_put_uint8_t(buf, 20, s_flag3);
|
|
|
+ _mav_put_float(buf, 12, flight_dist);
|
|
|
+ _mav_put_uint16_t(buf, 16, ups_volt);
|
|
|
+ _mav_put_uint16_t(buf, 18, adc_volt);
|
|
|
+ _mav_put_uint16_t(buf, 20, servo_state);
|
|
|
+ _mav_put_uint8_t(buf, 22, rtl_reason);
|
|
|
+ _mav_put_uint8_t(buf, 23, loiter_reason);
|
|
|
+ _mav_put_uint8_t(buf, 24, s_flag3);
|
|
|
|
|
|
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VKFMU_STATUS_LEN);
|
|
|
#else
|
|
@@ -207,6 +218,7 @@ static inline uint16_t mavlink_msg_vkfmu_status_pack_chan(uint8_t system_id, uin
|
|
|
packet.time_boot_ms = time_boot_ms;
|
|
|
packet.flight_time = flight_time;
|
|
|
packet.dist_to_tar = dist_to_tar;
|
|
|
+ packet.flight_dist = flight_dist;
|
|
|
packet.ups_volt = ups_volt;
|
|
|
packet.adc_volt = adc_volt;
|
|
|
packet.servo_state = servo_state;
|
|
@@ -231,7 +243,7 @@ static inline uint16_t mavlink_msg_vkfmu_status_pack_chan(uint8_t system_id, uin
|
|
|
*/
|
|
|
static inline uint16_t mavlink_msg_vkfmu_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vkfmu_status_t* vkfmu_status)
|
|
|
{
|
|
|
- return mavlink_msg_vkfmu_status_pack(system_id, component_id, msg, vkfmu_status->time_boot_ms, vkfmu_status->rtl_reason, vkfmu_status->loiter_reason, vkfmu_status->s_flag3, vkfmu_status->ups_volt, vkfmu_status->adc_volt, vkfmu_status->flight_time, vkfmu_status->dist_to_tar, vkfmu_status->servo_state);
|
|
|
+ return mavlink_msg_vkfmu_status_pack(system_id, component_id, msg, vkfmu_status->time_boot_ms, vkfmu_status->rtl_reason, vkfmu_status->loiter_reason, vkfmu_status->s_flag3, vkfmu_status->ups_volt, vkfmu_status->adc_volt, vkfmu_status->flight_time, vkfmu_status->dist_to_tar, vkfmu_status->servo_state, vkfmu_status->flight_dist);
|
|
|
}
|
|
|
|
|
|
/**
|
|
@@ -245,7 +257,7 @@ static inline uint16_t mavlink_msg_vkfmu_status_encode(uint8_t system_id, uint8_
|
|
|
*/
|
|
|
static inline uint16_t mavlink_msg_vkfmu_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vkfmu_status_t* vkfmu_status)
|
|
|
{
|
|
|
- return mavlink_msg_vkfmu_status_pack_chan(system_id, component_id, chan, msg, vkfmu_status->time_boot_ms, vkfmu_status->rtl_reason, vkfmu_status->loiter_reason, vkfmu_status->s_flag3, vkfmu_status->ups_volt, vkfmu_status->adc_volt, vkfmu_status->flight_time, vkfmu_status->dist_to_tar, vkfmu_status->servo_state);
|
|
|
+ return mavlink_msg_vkfmu_status_pack_chan(system_id, component_id, chan, msg, vkfmu_status->time_boot_ms, vkfmu_status->rtl_reason, vkfmu_status->loiter_reason, vkfmu_status->s_flag3, vkfmu_status->ups_volt, vkfmu_status->adc_volt, vkfmu_status->flight_time, vkfmu_status->dist_to_tar, vkfmu_status->servo_state, vkfmu_status->flight_dist);
|
|
|
}
|
|
|
|
|
|
/**
|
|
@@ -259,7 +271,7 @@ static inline uint16_t mavlink_msg_vkfmu_status_encode_chan(uint8_t system_id, u
|
|
|
*/
|
|
|
static inline uint16_t mavlink_msg_vkfmu_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_vkfmu_status_t* vkfmu_status)
|
|
|
{
|
|
|
- return mavlink_msg_vkfmu_status_pack_status(system_id, component_id, _status, msg, vkfmu_status->time_boot_ms, vkfmu_status->rtl_reason, vkfmu_status->loiter_reason, vkfmu_status->s_flag3, vkfmu_status->ups_volt, vkfmu_status->adc_volt, vkfmu_status->flight_time, vkfmu_status->dist_to_tar, vkfmu_status->servo_state);
|
|
|
+ return mavlink_msg_vkfmu_status_pack_status(system_id, component_id, _status, msg, vkfmu_status->time_boot_ms, vkfmu_status->rtl_reason, vkfmu_status->loiter_reason, vkfmu_status->s_flag3, vkfmu_status->ups_volt, vkfmu_status->adc_volt, vkfmu_status->flight_time, vkfmu_status->dist_to_tar, vkfmu_status->servo_state, vkfmu_status->flight_dist);
|
|
|
}
|
|
|
|
|
|
/**
|
|
@@ -275,22 +287,24 @@ static inline uint16_t mavlink_msg_vkfmu_status_encode_status(uint8_t system_id,
|
|
|
* @param flight_time [s] flight time in seconds
|
|
|
* @param dist_to_tar [cm] distance to target position in cm
|
|
|
* @param servo_state bitmap for servo state
|
|
|
+ * @param flight_dist [m] flight distance since this power up
|
|
|
*/
|
|
|
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
|
|
|
|
|
-static inline void mavlink_msg_vkfmu_status_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t rtl_reason, uint8_t loiter_reason, uint8_t s_flag3, uint16_t ups_volt, uint16_t adc_volt, uint32_t flight_time, uint32_t dist_to_tar, uint16_t servo_state)
|
|
|
+static inline void mavlink_msg_vkfmu_status_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t rtl_reason, uint8_t loiter_reason, uint8_t s_flag3, uint16_t ups_volt, uint16_t adc_volt, uint32_t flight_time, uint32_t dist_to_tar, uint16_t servo_state, float flight_dist)
|
|
|
{
|
|
|
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
|
|
char buf[MAVLINK_MSG_ID_VKFMU_STATUS_LEN];
|
|
|
_mav_put_uint32_t(buf, 0, time_boot_ms);
|
|
|
_mav_put_uint32_t(buf, 4, flight_time);
|
|
|
_mav_put_uint32_t(buf, 8, dist_to_tar);
|
|
|
- _mav_put_uint16_t(buf, 12, ups_volt);
|
|
|
- _mav_put_uint16_t(buf, 14, adc_volt);
|
|
|
- _mav_put_uint16_t(buf, 16, servo_state);
|
|
|
- _mav_put_uint8_t(buf, 18, rtl_reason);
|
|
|
- _mav_put_uint8_t(buf, 19, loiter_reason);
|
|
|
- _mav_put_uint8_t(buf, 20, s_flag3);
|
|
|
+ _mav_put_float(buf, 12, flight_dist);
|
|
|
+ _mav_put_uint16_t(buf, 16, ups_volt);
|
|
|
+ _mav_put_uint16_t(buf, 18, adc_volt);
|
|
|
+ _mav_put_uint16_t(buf, 20, servo_state);
|
|
|
+ _mav_put_uint8_t(buf, 22, rtl_reason);
|
|
|
+ _mav_put_uint8_t(buf, 23, loiter_reason);
|
|
|
+ _mav_put_uint8_t(buf, 24, s_flag3);
|
|
|
|
|
|
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VKFMU_STATUS, buf, MAVLINK_MSG_ID_VKFMU_STATUS_MIN_LEN, MAVLINK_MSG_ID_VKFMU_STATUS_LEN, MAVLINK_MSG_ID_VKFMU_STATUS_CRC);
|
|
|
#else
|
|
@@ -298,6 +312,7 @@ static inline void mavlink_msg_vkfmu_status_send(mavlink_channel_t chan, uint32_
|
|
|
packet.time_boot_ms = time_boot_ms;
|
|
|
packet.flight_time = flight_time;
|
|
|
packet.dist_to_tar = dist_to_tar;
|
|
|
+ packet.flight_dist = flight_dist;
|
|
|
packet.ups_volt = ups_volt;
|
|
|
packet.adc_volt = adc_volt;
|
|
|
packet.servo_state = servo_state;
|
|
@@ -317,7 +332,7 @@ static inline void mavlink_msg_vkfmu_status_send(mavlink_channel_t chan, uint32_
|
|
|
static inline void mavlink_msg_vkfmu_status_send_struct(mavlink_channel_t chan, const mavlink_vkfmu_status_t* vkfmu_status)
|
|
|
{
|
|
|
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
|
|
- mavlink_msg_vkfmu_status_send(chan, vkfmu_status->time_boot_ms, vkfmu_status->rtl_reason, vkfmu_status->loiter_reason, vkfmu_status->s_flag3, vkfmu_status->ups_volt, vkfmu_status->adc_volt, vkfmu_status->flight_time, vkfmu_status->dist_to_tar, vkfmu_status->servo_state);
|
|
|
+ mavlink_msg_vkfmu_status_send(chan, vkfmu_status->time_boot_ms, vkfmu_status->rtl_reason, vkfmu_status->loiter_reason, vkfmu_status->s_flag3, vkfmu_status->ups_volt, vkfmu_status->adc_volt, vkfmu_status->flight_time, vkfmu_status->dist_to_tar, vkfmu_status->servo_state, vkfmu_status->flight_dist);
|
|
|
#else
|
|
|
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VKFMU_STATUS, (const char *)vkfmu_status, MAVLINK_MSG_ID_VKFMU_STATUS_MIN_LEN, MAVLINK_MSG_ID_VKFMU_STATUS_LEN, MAVLINK_MSG_ID_VKFMU_STATUS_CRC);
|
|
|
#endif
|
|
@@ -331,19 +346,20 @@ static inline void mavlink_msg_vkfmu_status_send_struct(mavlink_channel_t chan,
|
|
|
is usually the receive buffer for the channel, and allows a reply to an
|
|
|
incoming message with minimum stack space usage.
|
|
|
*/
|
|
|
-static inline void mavlink_msg_vkfmu_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t rtl_reason, uint8_t loiter_reason, uint8_t s_flag3, uint16_t ups_volt, uint16_t adc_volt, uint32_t flight_time, uint32_t dist_to_tar, uint16_t servo_state)
|
|
|
+static inline void mavlink_msg_vkfmu_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t rtl_reason, uint8_t loiter_reason, uint8_t s_flag3, uint16_t ups_volt, uint16_t adc_volt, uint32_t flight_time, uint32_t dist_to_tar, uint16_t servo_state, float flight_dist)
|
|
|
{
|
|
|
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
|
|
|
char *buf = (char *)msgbuf;
|
|
|
_mav_put_uint32_t(buf, 0, time_boot_ms);
|
|
|
_mav_put_uint32_t(buf, 4, flight_time);
|
|
|
_mav_put_uint32_t(buf, 8, dist_to_tar);
|
|
|
- _mav_put_uint16_t(buf, 12, ups_volt);
|
|
|
- _mav_put_uint16_t(buf, 14, adc_volt);
|
|
|
- _mav_put_uint16_t(buf, 16, servo_state);
|
|
|
- _mav_put_uint8_t(buf, 18, rtl_reason);
|
|
|
- _mav_put_uint8_t(buf, 19, loiter_reason);
|
|
|
- _mav_put_uint8_t(buf, 20, s_flag3);
|
|
|
+ _mav_put_float(buf, 12, flight_dist);
|
|
|
+ _mav_put_uint16_t(buf, 16, ups_volt);
|
|
|
+ _mav_put_uint16_t(buf, 18, adc_volt);
|
|
|
+ _mav_put_uint16_t(buf, 20, servo_state);
|
|
|
+ _mav_put_uint8_t(buf, 22, rtl_reason);
|
|
|
+ _mav_put_uint8_t(buf, 23, loiter_reason);
|
|
|
+ _mav_put_uint8_t(buf, 24, s_flag3);
|
|
|
|
|
|
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VKFMU_STATUS, buf, MAVLINK_MSG_ID_VKFMU_STATUS_MIN_LEN, MAVLINK_MSG_ID_VKFMU_STATUS_LEN, MAVLINK_MSG_ID_VKFMU_STATUS_CRC);
|
|
|
#else
|
|
@@ -351,6 +367,7 @@ static inline void mavlink_msg_vkfmu_status_send_buf(mavlink_message_t *msgbuf,
|
|
|
packet->time_boot_ms = time_boot_ms;
|
|
|
packet->flight_time = flight_time;
|
|
|
packet->dist_to_tar = dist_to_tar;
|
|
|
+ packet->flight_dist = flight_dist;
|
|
|
packet->ups_volt = ups_volt;
|
|
|
packet->adc_volt = adc_volt;
|
|
|
packet->servo_state = servo_state;
|
|
@@ -385,7 +402,7 @@ static inline uint32_t mavlink_msg_vkfmu_status_get_time_boot_ms(const mavlink_m
|
|
|
*/
|
|
|
static inline uint8_t mavlink_msg_vkfmu_status_get_rtl_reason(const mavlink_message_t* msg)
|
|
|
{
|
|
|
- return _MAV_RETURN_uint8_t(msg, 18);
|
|
|
+ return _MAV_RETURN_uint8_t(msg, 22);
|
|
|
}
|
|
|
|
|
|
/**
|
|
@@ -395,7 +412,7 @@ static inline uint8_t mavlink_msg_vkfmu_status_get_rtl_reason(const mavlink_mess
|
|
|
*/
|
|
|
static inline uint8_t mavlink_msg_vkfmu_status_get_loiter_reason(const mavlink_message_t* msg)
|
|
|
{
|
|
|
- return _MAV_RETURN_uint8_t(msg, 19);
|
|
|
+ return _MAV_RETURN_uint8_t(msg, 23);
|
|
|
}
|
|
|
|
|
|
/**
|
|
@@ -405,7 +422,7 @@ static inline uint8_t mavlink_msg_vkfmu_status_get_loiter_reason(const mavlink_m
|
|
|
*/
|
|
|
static inline uint8_t mavlink_msg_vkfmu_status_get_s_flag3(const mavlink_message_t* msg)
|
|
|
{
|
|
|
- return _MAV_RETURN_uint8_t(msg, 20);
|
|
|
+ return _MAV_RETURN_uint8_t(msg, 24);
|
|
|
}
|
|
|
|
|
|
/**
|
|
@@ -415,7 +432,7 @@ static inline uint8_t mavlink_msg_vkfmu_status_get_s_flag3(const mavlink_message
|
|
|
*/
|
|
|
static inline uint16_t mavlink_msg_vkfmu_status_get_ups_volt(const mavlink_message_t* msg)
|
|
|
{
|
|
|
- return _MAV_RETURN_uint16_t(msg, 12);
|
|
|
+ return _MAV_RETURN_uint16_t(msg, 16);
|
|
|
}
|
|
|
|
|
|
/**
|
|
@@ -425,7 +442,7 @@ static inline uint16_t mavlink_msg_vkfmu_status_get_ups_volt(const mavlink_messa
|
|
|
*/
|
|
|
static inline uint16_t mavlink_msg_vkfmu_status_get_adc_volt(const mavlink_message_t* msg)
|
|
|
{
|
|
|
- return _MAV_RETURN_uint16_t(msg, 14);
|
|
|
+ return _MAV_RETURN_uint16_t(msg, 18);
|
|
|
}
|
|
|
|
|
|
/**
|
|
@@ -455,7 +472,17 @@ static inline uint32_t mavlink_msg_vkfmu_status_get_dist_to_tar(const mavlink_me
|
|
|
*/
|
|
|
static inline uint16_t mavlink_msg_vkfmu_status_get_servo_state(const mavlink_message_t* msg)
|
|
|
{
|
|
|
- return _MAV_RETURN_uint16_t(msg, 16);
|
|
|
+ return _MAV_RETURN_uint16_t(msg, 20);
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief Get field flight_dist from vkfmu_status message
|
|
|
+ *
|
|
|
+ * @return [m] flight distance since this power up
|
|
|
+ */
|
|
|
+static inline float mavlink_msg_vkfmu_status_get_flight_dist(const mavlink_message_t* msg)
|
|
|
+{
|
|
|
+ return _MAV_RETURN_float(msg, 12);
|
|
|
}
|
|
|
|
|
|
/**
|
|
@@ -470,6 +497,7 @@ static inline void mavlink_msg_vkfmu_status_decode(const mavlink_message_t* msg,
|
|
|
vkfmu_status->time_boot_ms = mavlink_msg_vkfmu_status_get_time_boot_ms(msg);
|
|
|
vkfmu_status->flight_time = mavlink_msg_vkfmu_status_get_flight_time(msg);
|
|
|
vkfmu_status->dist_to_tar = mavlink_msg_vkfmu_status_get_dist_to_tar(msg);
|
|
|
+ vkfmu_status->flight_dist = mavlink_msg_vkfmu_status_get_flight_dist(msg);
|
|
|
vkfmu_status->ups_volt = mavlink_msg_vkfmu_status_get_ups_volt(msg);
|
|
|
vkfmu_status->adc_volt = mavlink_msg_vkfmu_status_get_adc_volt(msg);
|
|
|
vkfmu_status->servo_state = mavlink_msg_vkfmu_status_get_servo_state(msg);
|