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);
}
void cbox_write(const char* text)
{
// blocking serial output to telemetry port
int16_t status;
status = queue_string(text);
if (status && (serial_interrupt_stopped == 1))
{
serial_interrupt_stopped = 0;
udb_serial_start_sending_data();
}
// wait for completion
while (!serial_interrupt_stopped)
{
}
}
// Prepare a line of serial output and start it sending
void send_debug_line(void)
{
db_index = 0;
#if (BOARD_TYPE != UDB4_BOARD)
float temp = 35 + (mpu_temp.value + 521.0) / 340.0;
#else
float temp = 0;
#endif
sprintf( debug_buffer, "%5u %li %li %li %5i %5.2f %5i %5i %5i %5i %5i %5i %5i %5i %5i\r\n",
udb_heartbeat_counter,
lat_gps.WW, long_gps.WW, alt_sl_gps.WW, ground_velocity_magnitudeXY, (double)temp,
rmat[0], rmat[1], rmat[2],
rmat[3], rmat[4], rmat[5],
rmat[6], rmat[7], rmat[8] ) ;
udb_serial_start_sending_data();
}
//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);
}
int main (void)
{
mcu_init();
// Set up the libraries
udb_init();
dcm_init();
udb_serial_set_rate(115200);
sprintf( debug_buffer, " tick lat long alt gspd temp rmat0 rmat1 rmat2 rmat3 rmat4 rmat5 rmat6 rmat7 rmat8 \r\n");
udb_serial_start_sending_data();
LED_GREEN = LED_OFF;
// Start it up!
udb_run(); // This never returns.
return 0;
}
void serial_output(char* format, ...)
{
va_list arglist;
va_start(arglist, format);
int16_t start_index = end_index;
int16_t remaining = SERIAL_BUFFER_SIZE - start_index;
if (remaining > 1)
{
int16_t wrote = vsnprintf((char*) (&serial_buffer[start_index]), (size_t) remaining, format, arglist);
end_index = start_index + wrote;
}
if (sb_index == 0)
{
udb_serial_start_sending_data();
}
va_end(arglist);
}
