void mp_mavlink_transmit(uint8_t ch) // routine to send a single character used by MAVlink standard include routines. // We forward to multi-byte sending routine so that firmware can interleave // ascii debug messages with MAVLink binary messages without them overwriting the buffer. { //printf("mp_mavlink_transmit(%u)\r\n", ch); mavlink_serial_send(MAVLINK_COMM_0, &ch, 1); }
void send_text(uint8_t text[]) { uint16_t index = 0; while (text[index++] != 0 && index < 80) { ; // Do nothing, just measuring the length of the text } //printf("send_text(%s) %u\r\n", text, index); mavlink_serial_send(MAVLINK_COMM_0, text, index - 1); }
// add printf library when running tests to output ascii messages of test results static void serial_output(const char* format, ...) { int16_t remaining = 0; int16_t wrote = 0; va_list arglist; va_start(arglist, format); remaining = MAVLINK_TEST_MESSAGE_SIZE; wrote = vsnprintf((char*)(&mavlink_test_message_buffer[0]), (size_t)remaining, format, arglist); if (wrote > 0) { mavlink_serial_send(MAVLINK_COMM_0, &mavlink_test_message_buffer[0], (uint16_t)wrote); // printf("%s\r\n", mavlink_test_message_buffer); } }
void TxN_Data_OverU1(uint16_t N){ #if (WIN != 1)//SLUGS2 SIL uint16_T i; for (i = 0U; i < N; i++) { uint16_T Tmp; Tmp = ~(MCHP_UART1_Tx.tail - MCHP_UART1_Tx.head); Tmp = Tmp & (Tx_BUFF_SIZE_Uart1 - 1);/* Modulo Buffer Size */ if (Tmp != 0) { MCHP_UART1_Tx.buffer[MCHP_UART1_Tx.tail] = UartOutBuff[i]; MCHP_UART1_Tx.tail = (MCHP_UART1_Tx.tail + 1) & (Tx_BUFF_SIZE_Uart1 - 1); Tmp--; } } _U1TXIF = U1STAbits.TRMT; #else mavlink_serial_send(MAVLINK_COMM_0, &UartOutBuff[0], (uint16_t)N); #endif }