diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index aa705b4229..24c1d0cb38 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -383,15 +383,6 @@ void Rover::send_current_waypoint(mavlink_channel_t chan) mavlink_msg_mission_current_send(chan, mission.get_current_nav_index()); } -void Rover::send_statustext(mavlink_channel_t chan) -{ - mavlink_statustext_t *s = &gcs[chan-MAVLINK_COMM_0].pending_status; - mavlink_msg_statustext_send( - chan, - s->severity, - s->text); -} - // are we still delaying telemetry to try to avoid Xbee bricking? bool Rover::telemetry_delayed(mavlink_channel_t chan) { @@ -521,9 +512,8 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) break; case MSG_STATUSTEXT: - CHECK_PAYLOAD_SIZE(STATUSTEXT); - rover.send_statustext(chan); - break; + // depreciated, use GCS_MAVLINK::send_statustext* + return false; case MSG_AHRS: CHECK_PAYLOAD_SIZE(AHRS); @@ -1452,14 +1442,7 @@ void Rover::gcs_update(void) void Rover::gcs_send_text(MAV_SEVERITY severity, const char *str) { - for (uint8_t i=0; ivsnprintf((char *)gcs[0].pending_status.text, - sizeof(gcs[0].pending_status.text), fmt, arg_list); + hal.util->vsnprintf((char *)str, sizeof(str), fmt, arg_list); va_end(arg_list); -#if LOGGING_ENABLED == ENABLED - DataFlash.Log_Write_Message(gcs[0].pending_status.text); -#endif - gcs[0].send_message(MSG_STATUSTEXT); - for (uint8_t i=1; i