void serial_output(char* format, ...)
{
char telebuf[200];
va_list arglist;
va_start(arglist, format);
int16_t len = vsnprintf(telebuf, sizeof (telebuf), format, arglist);
// static int maxlen = 0;
// if (len > maxlen) {
// maxlen = len;
// printf("maxlen %u\r\n", maxlen);
// }
int16_t start_index = end_index;
int16_t remaining = (SERIAL_BUFFER_SIZE - start_index);
if (remaining < len)
{
printf("SERBUF discarding %u bytes\r\n", len - remaining);
}
if (remaining > 1)
{
strncpy((char*) (&serial_buffer[start_index]), telebuf, MIN(remaining, len));
end_index = start_index + MIN(remaining, len);
serial_buffer[end_index] = '\0';
}
if (sb_index == 0)
{
udb_serial_start_sending_data();
}
log_telemetry(telebuf, len);
va_end(arglist);
}
//int16_t mavlink_serial_send(mavlink_channel_t UNUSED(chan), uint8_t buf[], uint16_t len)
int16_t mavlink_serial_send(mavlink_channel_t UNUSED(chan), const uint8_t buf[], uint16_t len) // RobD
// Note: Channel Number, chan, is currently ignored.
{
int16_t start_index;
int16_t remaining;
#if (USE_TELELOG == 1)
//printf("calling log_telemetry with %u bytes\r\n", len);
log_telemetry(buf, len);
#endif // USE_TELELOG
// Note at the moment, all channels lead to the one serial port
if (serial_interrupt_stopped == 1)
{
sb_index = 0;
end_index = 0;
}
start_index = end_index;
remaining = SERIAL_BUFFER_SIZE - start_index;
// printf("%u\r\n", remaining);
if (len > remaining)
{
// Chuck away the entire packet, as sending partial packet
// will break MAVLink CRC checks, and so receiver will throw it away anyway.
return (-1);
}
if (remaining > 1)
{
memcpy(&serial_buffer[start_index], buf, len);
end_index = start_index + len;
}
if (serial_interrupt_stopped == 1)
{
serial_interrupt_stopped = 0;
#if (SILSIM == 1)
mavlink_start_sending_data();
#else
udb_serial_start_sending_data();
#endif
}
return (1);
}
