|
|
|
@ -588,7 +588,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -588,7 +588,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
uint8_t result = MAV_RESULT_UNSUPPORTED; |
|
|
|
|
|
|
|
|
|
// do command
|
|
|
|
|
send_text_P(SEVERITY_LOW,PSTR("command received: ")); |
|
|
|
|
send_text_P(MAV_SEVERITY_WARNING,PSTR("command received: ")); |
|
|
|
|
|
|
|
|
|
switch(packet.command) { |
|
|
|
|
|
|
|
|
@ -798,7 +798,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -798,7 +798,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
// check if this is the HOME wp
|
|
|
|
|
if (packet.seq == 0) { |
|
|
|
|
tracker.set_home(tell_command); // New home in EEPROM
|
|
|
|
|
send_text_P(SEVERITY_LOW,PSTR("new HOME received")); |
|
|
|
|
send_text_P(MAV_SEVERITY_WARNING,PSTR("new HOME received")); |
|
|
|
|
waypoint_receiving = false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -890,7 +890,7 @@ void Tracker::mavlink_delay_cb()
@@ -890,7 +890,7 @@ void Tracker::mavlink_delay_cb()
|
|
|
|
|
} |
|
|
|
|
if (tnow - last_5s > 5000) { |
|
|
|
|
last_5s = tnow; |
|
|
|
|
gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM...")); |
|
|
|
|
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("Initialising APM...")); |
|
|
|
|
} |
|
|
|
|
in_mavlink_delay = false; |
|
|
|
|
} |
|
|
|
@ -931,7 +931,7 @@ void Tracker::gcs_update(void)
@@ -931,7 +931,7 @@ void Tracker::gcs_update(void)
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void Tracker::gcs_send_text_P(gcs_severity severity, const prog_char_t *str) |
|
|
|
|
void Tracker::gcs_send_text_P(MAV_SEVERITY severity, const prog_char_t *str) |
|
|
|
|
{ |
|
|
|
|
for (uint8_t i=0; i<num_gcs; i++) { |
|
|
|
|
if (gcs[i].initialised) { |
|
|
|
@ -951,7 +951,7 @@ void Tracker::gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
@@ -951,7 +951,7 @@ void Tracker::gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
|
|
|
|
|
void Tracker::gcs_send_text_fmt(const prog_char_t *fmt, ...) |
|
|
|
|
{ |
|
|
|
|
va_list arg_list; |
|
|
|
|
gcs[0].pending_status.severity = (uint8_t)SEVERITY_LOW; |
|
|
|
|
gcs[0].pending_status.severity = (uint8_t)MAV_SEVERITY_WARNING; |
|
|
|
|
va_start(arg_list, fmt); |
|
|
|
|
hal.util->vsnprintf_P((char *)gcs[0].pending_status.text, |
|
|
|
|
sizeof(gcs[0].pending_status.text), fmt, arg_list); |
|
|
|
|