void
GCS_MAVLINK::update(void (*run_cli)(AP_HAL::UARTDriver *))
{
// receive new packets
mavlink_message_t msg;
mavlink_status_t status;
status.packet_rx_drop_count = 0;
// process received bytes
uint16_t nbytes = comm_get_available(chan);
for (uint16_t i=0; i<nbytes; i++)
{
uint8_t c = comm_receive_ch(chan);
if (run_cli != NULL) {
/* allow CLI to be started by hitting enter 3 times, if no
* heartbeat packets have been received */
if (!mavlink_active && (hal.scheduler->millis() - _cli_timeout) < 20000 &&
comm_is_idle(chan)) {
if (c == '\n' || c == '\r') {
crlf_count++;
} else {
crlf_count = 0;
}
if (crlf_count == 3) {
run_cli(_port);
}
}
}
// Try to get a new message
if (mavlink_parse_char(chan, c, &msg, &status)) {
// we exclude radio packets to make it possible to use the
// CLI over the radio
if (msg.msgid != MAVLINK_MSG_ID_RADIO && msg.msgid != MAVLINK_MSG_ID_RADIO_STATUS) {
mavlink_active = true;
}
handleMessage(&msg);
}
}
if (!waypoint_receiving) {
return;
}
uint32_t tnow = hal.scheduler->millis();
uint32_t wp_recv_time = 1000U + (stream_slowdown*20);
if (waypoint_receiving &&
waypoint_request_i <= waypoint_request_last &&
tnow - waypoint_timelast_request > wp_recv_time) {
waypoint_timelast_request = tnow;
send_message(MSG_NEXT_WAYPOINT);
}
// stop waypoint receiving if timeout
if (waypoint_receiving && (tnow - waypoint_timelast_receive) > wp_recv_time+waypoint_receive_timeout) {
waypoint_receiving = false;
}
}
int main(int argc, char *const argv[])
{
int opt = 0, long_index = 0, status = 0;
FILE *lf = stderr; /**< Default logfile. */
char *lfpath = NULL;
cli_code cli_stat = CLI_PRE_INIT;
/* Parse command-line arguments. */
while ((opt = getopt_long(argc, argv,
g_short_opts, g_long_opts, &long_index)) != -1) {
switch (opt) {
case 'l':
if (!optarg) {
fputs("The logfile option requires a valid argument.\n",
stderr);
status = 1;
goto exit;
}
if (!(lf = freopen(optarg, "a+", stderr))) {
fprintf(stderr, "Could not open logfile '%s'!\n", optarg);
perror("logfile");
status = 2;
goto exit;
}
if (!(lfpath = strndup(optarg, strlen(optarg) + 1))) {
perror("logfile path strndup");
status = 3;
goto exit;
}
fprintf(stderr, "\t*** Logfile: '%s' ***\n", lfpath);
break;
case 'h':
usage();
goto exit;
default:
break;
}
}
if (!lfpath) {
char *tmpname = strdup("/tmp/chemtk_dbg_XXXXXX");
assert(tmpname);
int tmpfile = mkstemp(tmpname);
lfpath = tmpname;
if (tmpfile == -1) {
perror("mkstemp");
goto exit;
}
if (!(lf = freopen(tmpname, "a+", stderr))) {
fprintf(stderr, "Could not open logfile '%s'!\n", tmpname);
perror("logfile");
status = 2;
goto exit;
}
}
cli_stat = run_cli(stderr);
printf("CLI exited: %s\n", cli_statuses[cli_stat]);
if (cli_stat != CLI_OK)
status = 100 + cli_stat;
exit:
if (lf != stderr && lf != NULL)
fclose(lf);
if (lfpath) {
if (unlink(lfpath) == -1) {
fprintf(stderr, "Could not unlink '%s'.\n", lfpath);
perror("unlink");
}
}
free(lfpath);
delete_sc_stack(g_stack, free);
free_command_info();
return status;
}
