DEFINE_THREAD_ROUTINE_STACK(ATcodec_Commands_Client,data,ATCODEC_STACK_SIZE)
{
AT_CODEC_ERROR_CODE res;
int32_t v_loop;
v_continue = 1;
PRINT("Thread AT Commands Client Start\n");
while(!atcodec_lib_init_ok)
{
vp_os_thread_yield();
}
while(v_continue)
{
// open and init
if((res = func_ptrs.open()) != AT_CODEC_OPEN_OK)
v_continue = 0;
for ( v_loop = 1 ; v_loop ; )
{
// vp_os_delay(ATCODEC_SERVER_YIELD_DELAY);
vp_os_thread_yield();
// wait a successful ATcodec_Queue_... has been called
vp_os_mutex_lock(&ATcodec_cond_mutex);
//vp_os_cond_wait(&ATcodec_wait_cond);
//ATCODEC_PRINT("Must send \"%s\"\n", &ATcodec_Message_Buffer[0]);
if(ATcodec_Message_len && func_ptrs.write((int8_t*)&ATcodec_Message_Buffer[0], (int32_t*)&ATcodec_Message_len) != AT_CODEC_WRITE_OK)
v_loop = 0;
ATcodec_Message_len = 0;
vp_os_mutex_unlock(&ATcodec_cond_mutex);
}
// close and un-init : user-defined
if((res = func_ptrs.close()) != AT_CODEC_CLOSE_OK)
v_continue = 0;
}
if((res = func_ptrs.shutdown()) != AT_CODEC_SHUTDOWN_OK)
{
ATCODEC_PRINT("ATcodec Shutdown error\n");
}
return((THREAD_RET)0);
}
AT_CODEC_ERROR_CODE host_read(uint8_t *buffer, int32_t *len)
{
if( func_ptrs.read != NULL )
return func_ptrs.read( buffer, len );
return AT_CODEC_READ_OK;
}
void
ATcodec_Init_Library_Tree (ATcodec_Tree_t *tree, AT_CODEC_FUNCTIONS_PTRS *funcs)
{
VP_OS_ASSERT(funcs);
VP_OS_ASSERT(funcs->open);
VP_OS_ASSERT(funcs->read);
VP_OS_ASSERT(funcs->enable);
VP_OS_ASSERT(funcs->write);
VP_OS_ASSERT(funcs->close);
VP_OS_ASSERT(funcs->init);
VP_OS_ASSERT(funcs->shutdown);
ATcodec_Tree_init(tree, sizeof(ATcodec_Message_Data_t), 1);
memcpy(&func_ptrs, funcs, sizeof(*funcs));
vp_os_mutex_init(&ATcodec_cond_mutex);
vp_os_cond_init(&ATcodec_wait_cond, &ATcodec_cond_mutex);
if(func_ptrs.init() != AT_CODEC_INIT_OK)
{
ATCODEC_PRINT("ATcodec Init error\n");
}
else
{
ATcodec_Tree_print(tree);
atcodec_lib_init_ok = 1;
ATcodec_Message_len = 0;
}
}
AT_CODEC_ERROR_CODE host_enable( void )
{
if( func_ptrs.enable != NULL )
return func_ptrs.enable();
/* Only used with ARDrone */
return AT_CODEC_ENABLE_OK;
}
AT_CODEC_ERROR_CODE host_close( void )
{
if( func_ptrs.close != NULL )
return func_ptrs.close();
vp_com_close(COM_AT(), &at_socket);
return AT_CODEC_CLOSE_OK;
}
AT_CODEC_ERROR_CODE host_shutdown( void )
{
if( func_ptrs.shutdown != NULL )
func_ptrs.shutdown();
ardrone_at_shutdown();
return AT_CODEC_SHUTDOWN_OK;
}
AT_CODEC_ERROR_CODE host_write(uint8_t *buffer, int32_t *len)
{
if( func_ptrs.write != NULL )
return func_ptrs.write( buffer, len );
if( atcodec_write != NULL )
{
return VP_FAILED(atcodec_write(&at_socket, buffer, len)) ? AT_CODEC_WRITE_ERROR : AT_CODEC_WRITE_OK;
}
return AT_CODEC_WRITE_OK;
}
AT_CODEC_ERROR_CODE host_open( void )
{
static bool_t init_ok = FALSE;
if( func_ptrs.open != NULL )
return func_ptrs.open();
if( !init_ok )
{
COM_CONFIG_SOCKET_AT(&at_socket, VP_COM_CLIENT, AT_PORT, wifi_ardrone_ip);
at_socket.protocol = VP_COM_UDP;
if(VP_FAILED(vp_com_init(COM_AT())))
{
PRINT ("Failed to init AT\n");
vp_com_shutdown( COM_AT() );
return AT_CODEC_OPEN_ERROR;
}
if(VP_FAILED(vp_com_open(COM_AT(), &at_socket, &atcodec_read, &atcodec_write)))
{
PRINT ("Failed to open AT\n");
return AT_CODEC_OPEN_ERROR;
}
// set send_buffer to a low value to limit latency
int32_t sendbuf = AT_MUTEX_SNDBUF_SIZE;
if ( setsockopt((int32_t)at_socket.priv, SOL_SOCKET, SO_SNDBUF, &sendbuf, sizeof(sendbuf)) )
{
PRINT ("Error setting SND_BUF for AT socket\n");
}
int opt = IPTOS_PREC_NETCONTROL;
int res = setsockopt((int)at_socket.priv, IPPROTO_IP, IP_TOS, &opt, (socklen_t)sizeof(opt));
if (res)
{
perror("AT stage - setting Live video socket IP Type Of Service : ");
}
else
{
printf ("Set IP_TOS ok\n");
}
init_ok = TRUE;
}
return AT_CODEC_OPEN_OK;
}
ATCODEC_RET
ATcodec_Send_Messages()
{
ATCODEC_RET res = ATCODEC_TRUE;
if(!atcodec_lib_init_ok)
return ATCODEC_FALSE;
vp_os_mutex_lock(&ATcodec_cond_mutex);
if(ATcodec_Message_len > INTERNAL_BUFFER_SIZE)
printf("ATcodec_Send_Messages : buf=%s, len=%d\n", &ATcodec_Message_Buffer[0], ATcodec_Message_len);
if(ATcodec_Message_len && func_ptrs.write((uint8_t*)&ATcodec_Message_Buffer[0], (int32_t*)&ATcodec_Message_len) != AT_CODEC_WRITE_OK)
res = ATCODEC_FALSE;
ATcodec_Message_len = 0;
vp_os_mutex_unlock(&ATcodec_cond_mutex);
return res;
}
/********************************************************************
* Static functions
*******************************************************************/
AT_CODEC_ERROR_CODE host_init( void )
{
if( func_ptrs.init != NULL )
func_ptrs.init();
# undef ATCODEC_DEFINE_AT_CMD
# define ATCODEC_DEFINE_AT_CMD(ID,Str,From,Cb,Prio) \
if((ids.ID = ATcodec_Add_Defined_Message(Str)) == -1) \
{ \
return AT_CODEC_INIT_ERROR; \
}
# undef ATCODEC_DEFINE_AT_RESU
# define ATCODEC_DEFINE_AT_RESU(ID,Str,From,Cb) \
if((ids.ID = ATcodec_Add_Hashed_Message(Str,ids.From,Cb,0)) == -1) \
{ \
return AT_CODEC_INIT_ERROR; \
}
# include <at_msgs.h>
return AT_CODEC_INIT_OK;
}
static ATCODEC_RET
test_process_node(ATcodec_Tree_t *tree, ATcodec_Tree_Node_t *node, const char *cpy, ATcodec_Memory_t *memory, int *len_dec, int depth)
{
ATcodec_Tree_Node_t *son;
AT_CODEC_Message_Received ptr;
AT_CODEC_MSG_ID id = (AT_CODEC_MSG_ID)-1;
char *fmt_str;
int32_t output_len;
char output_params[INTERNAL_BUFFER_SIZE];
uint8_t output_str[INTERNAL_BUFFER_SIZE];
ATcodec_Memory_t output, dest, fmt;
if(test_dyn_strs(tree, node, depth, cpy, memory, &ptr, len_dec) == ATCODEC_TRUE)
{
ATcodec_Memory_Init(&output, &output_params[0], INTERNAL_BUFFER_SIZE, 1, NULL, NULL);
VP_OS_ASSERT(ptr);
ptr(memory, &output, &id);
if((int)id != -1)
{
son = ATcodec_Tree_Node_get(tree, id);
fmt_str = (char *)ATcodec_Buffer_getElement(&tree->strs, ((ATcodec_Message_Data_t *)ATcodec_Buffer_getElement(&tree->leaves, son->data))->total_str);
ATcodec_Memory_Init(&dest, (char*)&output_str[0], INTERNAL_BUFFER_SIZE, 1, NULL, NULL);
ATcodec_Memory_Init(&fmt, fmt_str, 0, 1, NULL, NULL);
output.current = (char *)output.start;
if(vp_atcodec_sprintf_params(&dest, &output_len, &fmt, &output) != ATCODEC_TRUE)
return ATCODEC_FALSE;
if(func_ptrs.write(&output_str[0], &output_len) != AT_CODEC_WRITE_OK)
return ATCODEC_FALSE;
}
return ATCODEC_TRUE;
}
return ATCODEC_FALSE;
}
AT_CODEC_ERROR_CODE host_open( void )
{
static bool_t init_ok = FALSE;
if( func_ptrs.open != NULL )
return func_ptrs.open();
if( !init_ok )
{
COM_CONFIG_SOCKET_AT(&at_socket, VP_COM_CLIENT, 0, wifi_ardrone_ip);
at_socket.protocol = VP_COM_UDP;
at_socket.remotePort = AT_PORT;
if(VP_FAILED(vp_com_init(COM_AT())))
{
PRINT ("Failed to init AT\n");
vp_com_shutdown( COM_AT() );
return AT_CODEC_OPEN_ERROR;
}
if(VP_FAILED(vp_com_open(COM_AT(), &at_socket, &atcodec_read, &atcodec_write)))
{
PRINT ("Failed to open AT\n");
return AT_CODEC_OPEN_ERROR;
}
// set send_buffer to a low value to limit latency
int32_t sendbuf = AT_MUTEX_SNDBUF_SIZE;
if ( setsockopt((int32_t)at_socket.priv, SOL_SOCKET, SO_SNDBUF, &sendbuf, sizeof(sendbuf)) )
{
PRINT ("Error setting SND_BUF for AT socket\n");
}
/*
* On android, with IP_TOS set, certain devices can't connect to AR.Drone 1
* So we just disable this functionnality to avoid these cases.
*/
#ifdef USE_ANDROID
if (IS_ARDRONE2)
{
#endif
int opt = IPTOS_PREC_NETCONTROL;
int res = setsockopt((int)at_socket.priv, IPPROTO_IP, IP_TOS, &opt, (socklen_t)sizeof(opt));
if (res)
{
perror("AT stage - setting Live video socket IP Type Of Service : ");
}
else
{
printf ("Set IP_TOS ok\n");
}
#ifdef USE_ANDROID
}
#endif
init_ok = TRUE;
}
return AT_CODEC_OPEN_OK;
}
/**
* AT Command receiving thread. ( ** AT Command Server ** )
* This thread keeps calling a callback function stored
* in 'func_ptrs.read' to fetch data, accumulates these data until
* a full AT command has been received, and then call the AT decoder.
* */
DEFINE_THREAD_ROUTINE_STACK(ATcodec_Commands_Server,data,ATCODEC_STACK_SIZE)
{
ATcodec_Tree_t *tree = &default_tree;
AT_CODEC_ERROR_CODE res;
int32_t v_loop, v_read, len, nb_cmd = 0;
char buffer[INTERNAL_BUFFER_SIZE]; // user-defined
char global_buffer[INTERNAL_BUFFER_SIZE];
char safety[16]; // Absorbs data overflowing from global_buffer.
int global_len=0;
v_continue = 1;
PRINT("Thread AT Commands Server Start\n");
while(!atcodec_lib_init_ok)
{
vp_os_thread_yield();
}
while(v_continue)
{
vp_os_memset(buffer,0,sizeof(buffer));
vp_os_memset(global_buffer,0,sizeof(global_buffer));global_len=0;
vp_os_memset(safety,0,sizeof(safety));
// open and init
if((res = func_ptrs.open()) != AT_CODEC_OPEN_OK){
v_continue = 0;
}
for ( v_loop = 1 ; v_loop && func_ptrs.enable() == AT_CODEC_ENABLE_OK; )
{
v_read = 1;
do
{
// wait so that thread can give the hand : delay user-defined / OS-dependent
//vp_os_thread_yield();
// -> we do a blocking read few lines after, thus
// other threads will be able to run during the I/O
/* In case of reading from packets, we clear the incoming buffer.
* Splitting AT commands into several packets would be a bad idea since packet order in not guaranteed in UDP.
*/
if (at_codec_reading_mode==ATCODEC_READ_FROM_PACKETS){
vp_os_memset(global_buffer,0,sizeof(global_buffer));
global_len=0;
}
/*
* Read some bytes; this function blocks until some data are made
* available by the VP_COM thread.
*/
len = sizeof(buffer); //INTERNAL_BUFFER_SIZE/*/2*/; // user-defined
res = func_ptrs.read((uint8_t*)&buffer[0], (int32_t*)&len);
if(res == AT_CODEC_READ_OK)
{
if(len > 0)
{
// process characters and update v_read
// \todo Do not use nb_cmd ?
/* Data are accumulated in the global buffer until at least one '\r' is found. */
if((nb_cmd = append_reception(&buffer[0], len, &global_buffer[0], &global_len,sizeof(global_buffer))) > 0)
{
v_read = 0;
}
else if(nb_cmd == -1) /* no \r found in the global_buffer*/
{
// a buffer overflow occurs
switch(at_codec_reading_mode)
{
case ATCODEC_READ_FROM_STREAM:
PRINT("AT Codec buffer was filled before a full AT commands was received.");
break;
case ATCODEC_READ_FROM_PACKETS:
PRINT("AT Codec received a packet with no complete AT command or buffer was too small to store the whole packet.");
break;
}
//ATCODEC_PRINT("Overflow\n");
/* In case of overflow, a TCP connection should be reinitialized in order to resynchronize
* the client and the server. Otherwise there is no way to find the beginning of the next AT Command.
* For a UDP connection, we assume all packets begin with an AT Command, and we just wait
* for the next packet to arrive.
*/
if (at_codec_reading_mode==ATCODEC_READ_FROM_STREAM) { v_loop = 0; }
}
else
{
v_read = 1;
}
}
else
{
if(len < 0)
{
ATCODEC_PRINT("read returns a neg length\n");
v_loop = 0;
}
}
}
else /* if (res == AT_CODEC_READ_OK) */
{
// an error occurred
ATCODEC_PRINT("an error occurs\n");
v_loop = 0;
}
}
while (v_read && v_loop);
// process what has been received if no error occurs
if(v_loop)
{
// ...
if(process_received_data(tree, nb_cmd, &global_buffer[0], &global_len) != ATCODEC_TRUE)
{
ATCODEC_PRINT("process_received returns false\n");
v_loop = 0;
}
}
} /*for*/
// close and un-init : user-defined
if((res = func_ptrs.close()) != AT_CODEC_CLOSE_OK)
v_continue = 0;
}/* while */
if ((res = func_ptrs.shutdown()) != AT_CODEC_SHUTDOWN_OK)
{
ATCODEC_PRINT("ATcodec Shutdown error\n");
}
return((THREAD_RET)0);
}
