|
|
|
@ -1022,12 +1022,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -1022,12 +1022,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: // MAV ID: 21
|
|
|
|
|
{ |
|
|
|
|
// mark the firmware version in the tlog
|
|
|
|
|
send_text(MAV_SEVERITY_WARNING, FIRMWARE_STRING); |
|
|
|
|
send_text(MAV_SEVERITY_INFO, FIRMWARE_STRING); |
|
|
|
|
|
|
|
|
|
#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION) |
|
|
|
|
send_text(MAV_SEVERITY_WARNING, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION); |
|
|
|
|
send_text(MAV_SEVERITY_INFO, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION); |
|
|
|
|
#endif |
|
|
|
|
send_text(MAV_SEVERITY_WARNING, "Frame: " FRAME_CONFIG_STRING); |
|
|
|
|
send_text(MAV_SEVERITY_INFO, "Frame: " FRAME_CONFIG_STRING); |
|
|
|
|
handle_param_request_list(msg); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
@ -1518,18 +1518,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -1518,18 +1518,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
case MAV_CMD_DO_SEND_BANNER: { |
|
|
|
|
result = MAV_RESULT_ACCEPTED; |
|
|
|
|
|
|
|
|
|
send_text(MAV_SEVERITY_WARNING, FIRMWARE_STRING); |
|
|
|
|
send_text(MAV_SEVERITY_INFO, FIRMWARE_STRING); |
|
|
|
|
|
|
|
|
|
#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION) |
|
|
|
|
send_text(MAV_SEVERITY_WARNING, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION); |
|
|
|
|
send_text(MAV_SEVERITY_INFO, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION); |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
send_text(MAV_SEVERITY_WARNING, "Frame: " FRAME_CONFIG_STRING); |
|
|
|
|
send_text(MAV_SEVERITY_INFO, "Frame: " FRAME_CONFIG_STRING); |
|
|
|
|
|
|
|
|
|
// send system ID if we can
|
|
|
|
|
char sysid[40]; |
|
|
|
|
if (hal.util->get_system_id(sysid)) { |
|
|
|
|
send_text(MAV_SEVERITY_WARNING, sysid); |
|
|
|
|
send_text(MAV_SEVERITY_INFO, sysid); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
break; |
|
|
|
@ -1841,12 +1841,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -1841,12 +1841,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
|
|
|
|
|
if (packet.idx >= copter.rally.get_rally_total() || |
|
|
|
|
packet.idx >= copter.rally.get_rally_max()) { |
|
|
|
|
send_text(MAV_SEVERITY_WARNING,"bad rally point message ID"); |
|
|
|
|
send_text(MAV_SEVERITY_NOTICE,"bad rally point message ID"); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (packet.count != copter.rally.get_rally_total()) { |
|
|
|
|
send_text(MAV_SEVERITY_WARNING,"bad rally point message count"); |
|
|
|
|
send_text(MAV_SEVERITY_NOTICE,"bad rally point message count"); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -1867,27 +1867,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -1867,27 +1867,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
|
|
|
|
|
//send a rally point to the GCS
|
|
|
|
|
case MAVLINK_MSG_ID_RALLY_FETCH_POINT: { |
|
|
|
|
//send_text(MAV_SEVERITY_CRITICAL, "## getting rally point in GCS_Mavlink.cpp 1"); // #### TEMP
|
|
|
|
|
//send_text(MAV_SEVERITY_INFO, "## getting rally point in GCS_Mavlink.cpp 1"); // #### TEMP
|
|
|
|
|
|
|
|
|
|
mavlink_rally_fetch_point_t packet; |
|
|
|
|
mavlink_msg_rally_fetch_point_decode(msg, &packet); |
|
|
|
|
|
|
|
|
|
//send_text(MAV_SEVERITY_CRITICAL, "## getting rally point in GCS_Mavlink.cpp 2"); // #### TEMP
|
|
|
|
|
//send_text(MAV_SEVERITY_INFO, "## getting rally point in GCS_Mavlink.cpp 2"); // #### TEMP
|
|
|
|
|
|
|
|
|
|
if (packet.idx > copter.rally.get_rally_total()) { |
|
|
|
|
send_text(MAV_SEVERITY_WARNING, "bad rally point index"); |
|
|
|
|
send_text(MAV_SEVERITY_NOTICE, "bad rally point index"); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
//send_text(MAV_SEVERITY_CRITICAL, "## getting rally point in GCS_Mavlink.cpp 3"); // #### TEMP
|
|
|
|
|
//send_text(MAV_SEVERITY_INFO, "## getting rally point in GCS_Mavlink.cpp 3"); // #### TEMP
|
|
|
|
|
|
|
|
|
|
RallyLocation rally_point; |
|
|
|
|
if (!copter.rally.get_rally_point_with_index(packet.idx, rally_point)) { |
|
|
|
|
send_text(MAV_SEVERITY_WARNING, "failed to set rally point"); |
|
|
|
|
send_text(MAV_SEVERITY_NOTICE, "failed to set rally point"); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
//send_text(MAV_SEVERITY_CRITICAL, "## getting rally point in GCS_Mavlink.cpp 4"); // #### TEMP
|
|
|
|
|
//send_text(MAV_SEVERITY_INFO, "## getting rally point in GCS_Mavlink.cpp 4"); // #### TEMP
|
|
|
|
|
|
|
|
|
|
mavlink_msg_rally_point_send_buf(msg, |
|
|
|
|
chan, msg->sysid, msg->compid, packet.idx, |
|
|
|
@ -1895,7 +1895,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -1895,7 +1895,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
rally_point.alt, rally_point.break_alt, rally_point.land_dir, |
|
|
|
|
rally_point.flags); |
|
|
|
|
|
|
|
|
|
//send_text(MAV_SEVERITY_CRITICAL, "## getting rally point in GCS_Mavlink.cpp 5"); // #### TEMP
|
|
|
|
|
//send_text(MAV_SEVERITY_INFO, "## getting rally point in GCS_Mavlink.cpp 5"); // #### TEMP
|
|
|
|
|
|
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
@ -1965,7 +1965,7 @@ void Copter::mavlink_delay_cb()
@@ -1965,7 +1965,7 @@ void Copter::mavlink_delay_cb()
|
|
|
|
|
} |
|
|
|
|
if (tnow - last_5s > 5000) { |
|
|
|
|
last_5s = tnow; |
|
|
|
|
gcs_send_text(MAV_SEVERITY_WARNING, "Initialising APM..."); |
|
|
|
|
gcs_send_text(MAV_SEVERITY_INFO, "Initialising APM..."); |
|
|
|
|
} |
|
|
|
|
check_usb_mux(); |
|
|
|
|
|
|
|
|
@ -2039,10 +2039,10 @@ void Copter::gcs_send_text(MAV_SEVERITY severity, const char *str)
@@ -2039,10 +2039,10 @@ void Copter::gcs_send_text(MAV_SEVERITY severity, const char *str)
|
|
|
|
|
* only one fits in the queue, so if you send more than one before the |
|
|
|
|
* last one gets into the serial buffer then the old one will be lost |
|
|
|
|
*/ |
|
|
|
|
void Copter::gcs_send_text_fmt(const char *fmt, ...) |
|
|
|
|
void Copter::gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...) |
|
|
|
|
{ |
|
|
|
|
va_list arg_list; |
|
|
|
|
gcs[0].pending_status.severity = (uint8_t)MAV_SEVERITY_WARNING; |
|
|
|
|
gcs[0].pending_status.severity = (uint8_t)severity; |
|
|
|
|
va_start(arg_list, fmt); |
|
|
|
|
hal.util->vsnprintf((char *)gcs[0].pending_status.text, |
|
|
|
|
sizeof(gcs[0].pending_status.text), fmt, arg_list); |
|
|
|
|