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); }