mirror of https://github.com/ArduPilot/ardupilot
GCS: added new gcs_send_text_fmt() method
this allows low priorty formatted messages to be sent to the ground station, without causing a CPU stall on the serial send buffer
This commit is contained in:
parent
f71674ffa4
commit
943c234c62
|
@ -3,6 +3,11 @@
|
||||||
// use this to prevent recursion during sensor init
|
// use this to prevent recursion during sensor init
|
||||||
static bool in_mavlink_delay;
|
static bool in_mavlink_delay;
|
||||||
|
|
||||||
|
// this costs us 51 bytes, but means that low priority
|
||||||
|
// messages don't block the CPU
|
||||||
|
static mavlink_statustext_t pending_status;
|
||||||
|
|
||||||
|
|
||||||
static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
|
static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
|
||||||
{
|
{
|
||||||
if (sysid != mavlink_system.sysid)
|
if (sysid != mavlink_system.sysid)
|
||||||
|
@ -276,6 +281,14 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
|
||||||
g.waypoint_index);
|
g.waypoint_index);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static void NOINLINE send_statustext(mavlink_channel_t chan)
|
||||||
|
{
|
||||||
|
mavlink_msg_statustext_send(
|
||||||
|
chan,
|
||||||
|
pending_status.severity,
|
||||||
|
pending_status.text);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
// try to send a message, return false if it won't fit in the serial tx buffer
|
// try to send a message, return false if it won't fit in the serial tx buffer
|
||||||
static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
|
static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
|
||||||
|
@ -391,6 +404,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case MSG_STATUSTEXT:
|
||||||
|
CHECK_PAYLOAD_SIZE(STATUSTEXT);
|
||||||
|
send_statustext(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
case MSG_RETRY_DEFERRED:
|
case MSG_RETRY_DEFERRED:
|
||||||
break; // just here to prevent a warning
|
break; // just here to prevent a warning
|
||||||
}
|
}
|
||||||
|
@ -460,22 +478,23 @@ static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uin
|
||||||
void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str)
|
void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str)
|
||||||
{
|
{
|
||||||
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
|
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
|
||||||
// don't send status MAVLink messages for 1 second after
|
// don't send status MAVLink messages for 2 seconds after
|
||||||
// bootup, to try to prevent Xbee bricking
|
// bootup, to try to prevent Xbee bricking
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (severity == SEVERITY_LOW &&
|
if (severity == SEVERITY_LOW) {
|
||||||
comm_get_txspace(chan) < MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) {
|
// send via the deferred queuing system
|
||||||
// don't send low priority messages unless they immediately
|
pending_status.severity = (uint8_t)severity;
|
||||||
// fit in the serial buffer
|
strncpy((char *)pending_status.text, str, sizeof(pending_status.text));
|
||||||
return;
|
mavlink_send_message(chan, MSG_STATUSTEXT, 0);
|
||||||
|
} else {
|
||||||
|
// send immediately
|
||||||
|
mavlink_msg_statustext_send(
|
||||||
|
chan,
|
||||||
|
severity,
|
||||||
|
(const int8_t*) str);
|
||||||
}
|
}
|
||||||
|
|
||||||
mavlink_msg_statustext_send(
|
|
||||||
chan,
|
|
||||||
severity,
|
|
||||||
(const int8_t*) str);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -1600,3 +1619,26 @@ static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
|
||||||
gcs0.send_text(severity, str);
|
gcs0.send_text(severity, str);
|
||||||
gcs3.send_text(severity, str);
|
gcs3.send_text(severity, str);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
send a low priority formatted message to the GCS
|
||||||
|
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
|
||||||
|
*/
|
||||||
|
static void gcs_send_text_fmt(const prog_char_t *fmt, ...)
|
||||||
|
{
|
||||||
|
char fmtstr[40];
|
||||||
|
va_list ap;
|
||||||
|
uint8_t i;
|
||||||
|
for (i=0; i<sizeof(fmtstr)-1; i++) {
|
||||||
|
fmtstr[i] = pgm_read_byte((const prog_char *)(fmt++));
|
||||||
|
if (fmtstr[i] == 0) break;
|
||||||
|
}
|
||||||
|
fmtstr[i] = 0;
|
||||||
|
pending_status.severity = (uint8_t)SEVERITY_LOW;
|
||||||
|
va_start(ap, fmt);
|
||||||
|
vsnprintf((char *)pending_status.text, sizeof(pending_status.text), fmtstr, ap);
|
||||||
|
va_end(ap);
|
||||||
|
mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0);
|
||||||
|
mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0);
|
||||||
|
}
|
||||||
|
|
|
@ -128,6 +128,7 @@ enum ap_message {
|
||||||
MSG_SERVO_OUT,
|
MSG_SERVO_OUT,
|
||||||
MSG_NEXT_WAYPOINT,
|
MSG_NEXT_WAYPOINT,
|
||||||
MSG_NEXT_PARAM,
|
MSG_NEXT_PARAM,
|
||||||
|
MSG_STATUSTEXT,
|
||||||
MSG_RETRY_DEFERRED // this must be last
|
MSG_RETRY_DEFERRED // this must be last
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue