diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 2e1553b7dd..a01fd73646 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -822,6 +822,20 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) break; } + case MAVLINK_MSG_ID_STATUSTEXT: + { + // ignore any statustext messages not from our GCS: + if (msg->sysid != rover.g.sysid_my_gcs) { + break; + } + mavlink_statustext_t packet; + mavlink_msg_statustext_decode(msg, &packet); + char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1+4] = { 'G','C','S',':'}; + memcpy(&text[4], packet.text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN); + rover.DataFlash.Log_Write_Message(text); + break; + } + case MAVLINK_MSG_ID_COMMAND_INT: { // decode packet mavlink_command_int_t packet;