diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 66f6da3405..1d272b8f38 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -290,6 +290,7 @@ protected: // message sending functions: bool try_send_compass_message(enum ap_message id); bool try_send_mission_message(enum ap_message id); + void send_hwstatus(); private: diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 18f858e4fd..c6211e8ae3 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -2085,6 +2085,13 @@ bool GCS_MAVLINK::try_send_mission_message(const enum ap_message id) return ret; } +void GCS_MAVLINK::send_hwstatus() +{ + mavlink_msg_hwstatus_send( + chan, + hal.analogin->board_voltage()*1000, + 0); +} bool GCS_MAVLINK::try_send_message(const enum ap_message id) { @@ -2096,6 +2103,12 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) switch(id) { + case MSG_HWSTATUS: + CHECK_PAYLOAD_SIZE(HWSTATUS); + send_hwstatus(); + ret = true; + break; + case MSG_CURRENT_WAYPOINT: /* fall through */ case MSG_MISSION_ITEM_REACHED: