static void flush_serial()
{
// Flush all data in serial port
ser_set_timeout_ms( ser, SER_NO_TIMEOUT );
while( ser_read_byte( ser ) != -1 );
ser_set_timeout_ms( ser, SER_INF_TIMEOUT );
}
// Open the serial port
ser_handler ser_open( const char* sername )
{
char portname[ WIN_MAX_PORT_NAME + 1 ];
HANDLE hComm;
ser_handler hnd;
portname[ WIN_MAX_PORT_NAME ] = '\0';
_snprintf( portname, WIN_MAX_PORT_NAME, "\\\\.\\%s", sername );
hComm = CreateFile( portname, GENERIC_READ | GENERIC_WRITE, 0, 0, OPEN_EXISTING, FILE_FLAG_OVERLAPPED, 0 );
if( hComm == INVALID_HANDLE_VALUE )
return SER_HANDLER_INVALID;
if( !SetupComm( hComm, 2048, 2048 ) )
return SER_HANDLER_INVALID;
if( ser_set_timeout_ms( hComm, SER_INF_TIMEOUT ) != SER_OK )
return SER_HANDLER_INVALID;
if( ( hnd = malloc( sizeof( SERIAL_DATA ) ) ) == NULL )
return SER_HANDLER_INVALID;
memset( hnd, 0, sizeof( SERIAL_DATA ) );
hnd->hnd = hComm;
hnd->fWaitingOnRead = FALSE;
if( ( hnd->o.hEvent = CreateEvent( NULL, TRUE, FALSE, NULL ) ) == NULL )
{
free( hnd );
return SER_HANDLER_INVALID;
}
if( ( hnd->o_wr.hEvent = CreateEvent( NULL, TRUE, FALSE, NULL ) ) == NULL )
{
free( hnd );
CloseHandle( hnd->o.hEvent );
return SER_HANDLER_INVALID;
}
return hnd;
}
void transport_open( Transport *tpt, const char *path )
{
struct exception e;
tpt->fd = ser_open( path );
if( tpt->fd == INVALID_TRANSPORT)
{
e.errnum = transport_errno;
e.type = fatal;
Throw( e );
}
ser_setup( tpt->fd, 115200, SER_DATABITS_8, SER_PARITY_NONE, SER_STOPBITS_1 );
ser_set_timeout_ms( tpt->fd, 1000 );
}
BOOL msg_get_resp( uint8_t chan, mavlink_message_t *p_msg, uint32_t timeout)
{
BOOL ret = FALSE;
int ch_ret;
uint8_t ch;
static mavlink_message_t msg[MSG_CH_MAX];
static mavlink_status_t status[MSG_CH_MAX];
uint32_t retry = timeout;
ser_set_timeout_ms( stm32_ser_id, 1 );
while(1)
{
ch_ret = read_byte();
if( ch_ret < 0 )
{
if( retry-- <= 0 )
{
ret = FALSE;
break;
}
else
{
continue;
}
}
else
{
ch = (uint8_t)(ch_ret);
retry = timeout;
}
ret = msg_recv( chan, ch, &msg[chan], &status[chan] );
if( ret == TRUE )
{
*p_msg = msg[chan];
break;
}
}
return ret;
}
