diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index c4a11f666d55d..19015a8f194d0 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -121,6 +121,17 @@ void GCS::send_named_float(const char *name, float value) const (const char *)&packet); } +void GCS::send_named_int(const char *name, int32_t value) const +{ + mavlink_named_value_int_t packet {}; + packet.time_boot_ms = AP_HAL::millis(); + packet.value = value; + memcpy(packet.name, name, MIN(strlen(name), (uint8_t)MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN)); + + gcs().send_to_active_channels(MAVLINK_MSG_ID_NAMED_VALUE_INT, + (const char *)&packet); +} + #if HAL_HIGH_LATENCY2_ENABLED void GCS::enable_high_latency_connections(bool enabled) { diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index aa67f61e8cedd..82406da66526e 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -388,6 +388,7 @@ class GCS_MAVLINK void send_gimbal_manager_information() const; void send_gimbal_manager_status() const; void send_named_float(const char *name, float value) const; + void send_named_int(const char *name, int32_t value) const; void send_home_position() const; void send_gps_global_origin() const; virtual void send_attitude_target() {}; @@ -1215,6 +1216,7 @@ class GCS void send_message(enum ap_message id); void send_mission_item_reached_message(uint16_t mission_index); void send_named_float(const char *name, float value) const; + void send_named_int(const char *name, int32_t value) const; void send_parameter_value(const char *param_name, ap_var_type param_type, diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 83eb282ac0274..bceab90f55835 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3024,6 +3024,13 @@ void GCS_MAVLINK::send_named_float(const char *name, float value) const mavlink_msg_named_value_float_send(chan, AP_HAL::millis(), float_name, value); } +void GCS_MAVLINK::send_named_int(const char *name, int32_t value) const +{ + char int_name[MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN+1] {}; + strncpy(int_name, name, MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN); + mavlink_msg_named_value_int_send(chan, AP_HAL::millis(), int_name, value); +} + #if AP_AHRS_ENABLED void GCS_MAVLINK::send_home_position() const {