// \todo Does source string need to be passed like an ATcodec_Memory_t ?
void
ATcodec_Memory_Put_String (ATcodec_Memory_t *mem, const char *str)
{
char *s = (char *)str;
VP_OS_ASSERT(s);
ATCODEC_MEMORY_PUT_STRUCT_ASSERT(mem);
if(*s)
{
do
{
ATCODEC_MEMORY_SIZE_ASSERT(mem, 1);
mem->put(&mem->current, *s++);
}
while ( *s );
}
}
C_RESULT vp_com_local_config(vp_com_t* vp_com, vp_com_config_t* config)
{
C_RESULT res = C_OK;
VP_OS_ASSERT( vp_com != NULL );
if( vp_com->config != config )
{
res = VP_COM_LOCAL_CONFIG(config);
if( SUCCEED( res ) )
vp_com->config = config;
}
return res;
}
C_RESULT vp_com_disconnect(vp_com_t* vp_com)
{
C_RESULT res = VP_COM_ERROR;
VP_OS_ASSERT( vp_com != NULL );
if(vp_com->config != NULL && vp_com->connection != NULL)
{
vp_os_mutex_lock(&vp_com->mutex);
res = VP_COM_DISCONNECT(vp_com->config, vp_com->connection);
if( SUCCEED( res ) )
vp_com->connection->is_up = 0;
vp_os_mutex_unlock(&vp_com->mutex);
}
return res;
}
C_RESULT vp_com_shutdown(vp_com_t* vp_com)
{
VP_OS_ASSERT( vp_com != NULL );
vp_os_mutex_lock( &vp_com->mutex );
if(vp_com->ref_count > 0)
{
vp_com->ref_count--;
if(vp_com->ref_count == 0)
{
vp_os_mutex_unlock( &vp_com->mutex );
vp_os_mutex_destroy( &vp_com->mutex );
return VP_COM_SHUTDOWN();
}
}
vp_os_mutex_unlock(&vp_com->mutex);
return VP_COM_OK;
}
AT_CODEC_MSG_ID
ATcodec_Add_Hashed_Message_Tree (ATcodec_Tree_t *tree, const char *str, AT_CODEC_MSG_ID from_cmd, AT_CODEC_Message_Received func_ptr, int priority)
{
ATcodec_Message_Data_t data, *p_data;
ATcodec_Tree_Node_t *node;
int len_s, len_d, node_i;
char buffer[1024];
data.static_str = -1;
data.dynamic_str = -1;
data.total_str = -1;
data.from_cmd = from_cmd;
data.func_ptr = func_ptr;
data.priority = priority;
/* Retrieves the length of the never-changing part of the AT command */
len_s = static_len(str);
VP_OS_ASSERT(len_s < 1024);
vp_os_memcpy(&buffer[0], str, len_s);
buffer[len_s] = 0;
node_i = ATcodec_Tree_insert(tree, &buffer[0], &data);
node = ATcodec_Tree_Node_get(tree, node_i);
p_data = ATcodec_Buffer_getElement(&tree->leaves, node->data);
p_data->static_str = node->strkey;
// backup dynamic_str
p_data->dynamic_str = tree->strs.nbElements;
len_d = strlen(str+len_s)+1;
ATcodec_Buffer_pushElements (&tree->strs, str+len_s, len_d);
// backup concatenation of static_str and dynamic_str
p_data->total_str = tree->strs.nbElements;
ATcodec_Buffer_pushElements (&tree->strs, str, len_s+len_d);
return (AT_CODEC_MSG_ID)node_i;
}
C_RESULT ardrone_tool_input_remove( input_device_t* device )
{
C_RESULT res;
int32_t i;
VP_OS_ASSERT( device != NULL );
res = C_FAIL;
i = 0;
while( i < MAX_NUM_DEVICES && devices[i] != device ) i++;
if( i < MAX_NUM_DEVICES )
{
res = ardrone_tool_input_remove_i(i);
}
else
{
DEBUG_PRINT_SDK("Input %s not found while removing\n", device->name);
}
return res;
}
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;
}
C_RESULT vp_com_connect(vp_com_t* vp_com, vp_com_connection_t* connection, uint32_t numAttempts)
{
C_RESULT res = VP_COM_OK;
bool_t already_connected;
VP_OS_ASSERT( vp_com != NULL );
if(vp_com->config != NULL)
{
vp_os_mutex_lock(&vp_com->mutex);
already_connected = vp_com->connection && vp_com->connection->is_up == 1;
// TODO voir pour ajouter un test sur l'adresse ethernet de la connection
if( already_connected && vp_com->connection != connection )
{
already_connected = FALSE;
vp_com_disconnect(vp_com);
}
if( !already_connected )
{
res = VP_COM_CONNECT(vp_com, connection, numAttempts);
if( SUCCEED( res ) )
{
vp_com->connection = connection;
vp_com->connection->is_up = 1;
}
}
vp_os_mutex_unlock(&vp_com->mutex);
}
return res;
}
void uvlc_encode( video_stream_t* const stream, int32_t level, int32_t run, int32_t not_last )
{
int32_t sign, length, data, value_code, value_length;
/// Encode number of zeros
data = run;
length = 0;
value_code = 1;
if( data > 0 )
{
length = 32 - clz(data); // compute number of bits used in run ( = length of run )
data -= 1 << ( length - 1 ); // compute value of run
}
value_length = length + 1;
length -= 1;
if( length > 0 )
{
PACK_BITS( value_code, value_length, data, length );
}
/// Encode level
data = level;
// sign handling
sign = 0;
if( data < 0 )
{
data = -data;
sign = 1;
}
// TODO Check saturation & if level == -128
length = 32 - clz(data); // number of bits used in level ( = length of level )
if( length > 1 )
{
data -= 1 << (length - 1);
length += 1;
}
PACK_BITS( value_code, value_length, 1, length );
VP_OS_ASSERT( length != 2 );
length -= 2;
if(length > 0)
{
PACK_BITS( value_code, value_length, data, length );
}
PACK_BITS( value_code, value_length, sign, 1 );
/// Encode last
// add sequence for end of block if required
if( not_last == 0 )
{
PACK_BITS( value_code, value_length, 0x5, 3 );
}
/// Write output
video_write_data( stream, value_code, value_length );
}
C_RESULT ardrone_academy_navdata_process( const navdata_unpacked_t* const pnd )
{
static bool_t prevCameraIsReady = FALSE;
static bool_t prevDroneUsbRecording = FALSE;
if (C_OK == TRYLOCK_ND_MUTEX ())
{
input_state_t* input_state = ardrone_tool_input_get_state();
bool_t recordIsReady = ! ardrone_academy_navdata_get_record_ready();
bool_t recordIsInProgress = ardrone_academy_navdata_get_record_state();
// Save ARDrone State
navdata_state.lastState = pnd->ardrone_state;
// Save remaining USB time
navdata_state.usbFreeTime = pnd->navdata_hdvideo_stream.usbkey_remaining_time;
bool_t currentDroneUsbRecording = (NAVDATA_HDVIDEO_USBKEY_IS_RECORDING == (NAVDATA_HDVIDEO_USBKEY_IS_RECORDING & pnd->navdata_hdvideo_stream.hdvideo_state));
// Check for record stop from drone
if (navdata_state.usbRecordInProgress)
{
if (TRUE == prevDroneUsbRecording &&
FALSE == currentDroneUsbRecording)
{
navdata_state.droneStoppedUsbRecording = TRUE;
navdata_state.record_state = RECORD_STATE_NEEDED;
}
}
prevDroneUsbRecording = currentDroneUsbRecording;
ardrone_academy_check_take_off_timeout ();
// Take off and Emergency management
if(navdata_state.takeoff_state == TAKEOFF_STATE_NEEDED)
{
if(pnd->ardrone_state & ARDRONE_EMERGENCY_MASK)
{
navdata_state.needSetEmergency = TRUE;
}
else
{
if(recordIsInProgress)
{
if(!(pnd->ardrone_state & ARDRONE_USER_FEEDBACK_START))
ardrone_tool_set_ui_pad_start(1);
else
ardrone_tool_set_ui_pad_start(0);
navdata_state.takeoff_state = TAKEOFF_STATE_IDLE;
}
else
{
navdata_state.takeoff_state = TAKEOFF_STATE_WAIT_USERBOX;
ardrone_academy_navdata_userbox_switch();
}
}
}
if(navdata_state.needSetEmergency)
{
ardrone_tool_set_ui_pad_select(1);
navdata_state.needSetEmergency = FALSE;
}
if(pnd->ardrone_state & ARDRONE_EMERGENCY_MASK)
{
if(!navdata_state.wasEmergency && (input_state->user_input & (1 << ARDRONE_UI_BIT_SELECT)))
{
ardrone_tool_set_ui_pad_select(0);
}
navdata_state.isEmergency = TRUE;
navdata_state.isTakeOff = FALSE;
if(!navdata_state.internalRecordInProgress && !navdata_state.usbRecordInProgress && (navdata_state.userbox_state == USERBOX_STATE_STARTED))
{
PRINT("Emergency !! Stopping userbox...\n");
ardrone_academy_navdata_userbox_switch();
}
ardrone_tool_input_start_reset();
}
else // Not emergency landing
{
if(navdata_state.wasEmergency && (input_state->user_input & (1 << ARDRONE_UI_BIT_SELECT)))
{
ardrone_tool_set_ui_pad_select(0);
}
if(!(pnd->ardrone_state & ARDRONE_TIMER_ELAPSED))
navdata_state.isEmergency = FALSE;
if(input_state->user_input & (1 << ARDRONE_UI_BIT_START))
{
navdata_state.isTakeOff = (pnd->ardrone_state & ARDRONE_USER_FEEDBACK_START) ? TRUE : FALSE;
}
else
{
navdata_state.isTakeOff = FALSE;
}
}
navdata_state.wasEmergency = (pnd->ardrone_state & ARDRONE_EMERGENCY_MASK) ? TRUE : FALSE;
// Video record management
bool_t usbRecordEnable = FALSE;
if(navdata_state.record_state == RECORD_STATE_NEEDED &&
TAKEOFF_STATE_IDLE == navdata_state.takeoff_state)
{
bool_t continueRecord = TRUE;
bool_t nextInternalRecordState = FALSE;
if (IS_LEAST_ARDRONE2)
{
static codec_type_t oldCodec;
codec_type_t cancelCodec;
if (recordIsReady && ! navdata_state.usbRecordInProgress && !navdata_state.internalRecordInProgress) // We want to enable recording
{
if ((ARDRONE_USB_MASK == (pnd->ardrone_state & ARDRONE_USB_MASK)) &&
(0 < pnd->navdata_hdvideo_stream.usbkey_remaining_time) &&
(TRUE == ardrone_control_config.video_on_usb))
{
usbRecordEnable = TRUE;
}
// Set the "non-record codec" to the codec defined as the application default
oldCodec = ardrone_application_default_config.video_codec;
cancelCodec = oldCodec;
ardrone_control_config.video_codec = (TRUE == usbRecordEnable) ? usbRecordCodec : deviceWifiRecordCodec;
PRINT ("Set codec %d -> %d\n", oldCodec, ardrone_control_config.video_codec);
nextInternalRecordState = TRUE;
}
else // We want to disable recording
{
cancelCodec = ardrone_control_config.video_codec;
ardrone_control_config.video_codec = oldCodec;
PRINT ("Reset codec %d -> %d\n", cancelCodec, oldCodec);
}
bool_t addEventSucceeded = ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_codec, &ardrone_control_config.video_codec, NULL);
if (FALSE == addEventSucceeded)
{
PRINT ("Unable to send codec switch ... retry later\n");
ardrone_control_config.video_codec = cancelCodec;
continueRecord = FALSE;
}
}
else if (IS_ARDRONE1)
{
nextInternalRecordState = !recordIsInProgress;
}
if (TRUE == continueRecord)
{
navdata_state.internalRecordInProgress = nextInternalRecordState;
switch (navdata_state.userbox_state)
{
case USERBOX_STATE_STOPPED:
navdata_state.record_state = RECORD_STATE_WAIT_USERBOX;
navdata_state.usbRecordInProgress = usbRecordEnable;
ardrone_academy_navdata_userbox_switch();
break;
case USERBOX_STATE_STARTED:
if(navdata_state.isTakeOff)
{
if(! recordIsReady)
{
PRINT("Userbox is started and record is started => Stop record\n");
ardrone_academy_navdata_recorder_enable(FALSE, navdata_state.userbox_time);
navdata_state.usbRecordInProgress = FALSE;
}
else
{
PRINT("Userbox is started and record is stopped => Start record\n");
if (FALSE == usbRecordEnable)
{
// Only activate the local recorder if we don't record on USB
ardrone_academy_navdata_recorder_enable(TRUE, navdata_state.userbox_time);
navdata_state.usbRecordInProgress = FALSE;
}
else
{
navdata_state.usbRecordInProgress = TRUE;
}
}
navdata_state.record_state = RECORD_STATE_IDLE;
}
else
{
navdata_state.record_state = RECORD_STATE_WAIT_USERBOX;
ardrone_academy_navdata_userbox_switch();
}
break;
case USERBOX_STATE_STARTING:
navdata_state.usbRecordInProgress = usbRecordEnable;
navdata_state.record_state = RECORD_STATE_WAIT_USERBOX;
break;
case USERBOX_STATE_STOPPING:
navdata_state.usbRecordInProgress = usbRecordEnable;
// Should never be here
PRINT ("Don't know what to do for USERBOX_STATE_STOPPING\n");
VP_OS_ASSERT (0 == 1); // Debug handler
break;
}
}
}
// Screenshot management
prevCameraIsReady = navdata_state.cameraIsReady;
navdata_state.cameraIsReady = (pnd->ardrone_state & ARDRONE_CAMERA_MASK) ? TRUE : FALSE;
if (TRUE == navdata_state.cameraIsReady && FALSE == prevCameraIsReady)
{
academy_download_resume ();
}
if((navdata_state.screenshot_state == SCREENSHOT_STATE_NEEDED) && navdata_state.cameraIsReady)
{
static char param[ARDRONE_DATE_MAXSIZE + 64];
static char date[ARDRONE_DATE_MAXSIZE];
time_t t = time(NULL);
ardrone_time2date(t, ARDRONE_FILE_DATE_FORMAT, date);
navdata_state.screenshot_state = SCREENSHOT_STATE_INPROGRESS;
sprintf(param, "%d,%d,%d,%s", USERBOX_CMD_SCREENSHOT, 0, 0, date);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(userbox_cmd, param, ardrone_academy_navdata_screenshot_cb);
}
// USB management
navdata_state.usbIsReady = (pnd->ardrone_state & ARDRONE_USB_MASK) ? TRUE : FALSE;
UNLOCK_ND_MUTEX ();
}
return C_OK;
}
C_RESULT vp_com_serial_open(vp_com_serial_config_t* config, vp_com_connection_t* connection, vp_com_socket_t* socket, Read* read, Write* write)
{
struct termios tio;
speed_t speed;
VP_OS_ASSERT(config->blocking == 0 || config->blocking == 1);
if(config->blocking == 0)
socket->priv = (void *)open(&config->itfName[0], O_RDWR|O_NOCTTY|O_NONBLOCK);
else
socket->priv = (void *)open(&config->itfName[0], O_RDWR|O_NOCTTY);
if(((int)socket->priv) == -1)
{
PRINT("Unable to \"open\" serial device");
return (VP_COM_ERROR);
}
/* get current serial port settings */
if(tcgetattr((int)socket->priv, &tio) != 0)
{
PRINT("Serial device configuration failure (%s)", strerror(errno));
close((int)socket->priv);
return (VP_COM_ERROR);
}
tio_save = tio;
if(config->sync)
{
/* set serial settings */
speed = (speed_t)config->initial_baudrate;
cfsetispeed(&tio, speed);
cfsetospeed(&tio, speed);
cfmakeraw(&tio);
if(tcsetattr((int)socket->priv, TCSANOW, &tio) != 0)
{
PRINT("Serial device configuration failure (%s)", strerror(errno));
close((int)socket->priv);
return (VP_COM_ERROR);
}
if(socket->type == VP_COM_CLIENT)
{
if(FAILED(vp_com_serial_write_sync(config, socket)))
return (VP_COM_ERROR);
vp_os_delay(VP_COM_SYNC_DELAY);
}
else if(socket->type == VP_COM_SERVER)
{
if(FAILED(vp_com_serial_wait_sync(config, socket)))
return (VP_COM_ERROR);
vp_os_delay(VP_COM_SYNC_DELAY);
}
}
/* set serial settings */
speed = (speed_t)config->baudrate;
cfsetispeed(&tio, speed);
cfsetospeed(&tio, speed);
cfmakeraw(&tio);
if(tcsetattr((int)socket->priv, TCSANOW, &tio) != 0)
{
PRINT("Serial device configuration failure (%s)", strerror(errno));
close((int)socket->priv);
return (VP_COM_ERROR);
}
if(read) *read = (Read) vp_com_serial_read;
if(write) *write = (Write) vp_com_serial_write;
return (VP_COM_OK);
}
C_RESULT vp_com_init(vp_com_t* vp_com)
{
C_RESULT res = VP_COM_NOTSUPPORTED;
VP_OS_ASSERT( vp_com != NULL );
if(!vp_com->initialized)
{
vp_os_mutex_init(&vp_com->mutex);
vp_com->initialized ++;
}
vp_os_mutex_lock(&vp_com->mutex);
if(vp_com->ref_count > 0)
{
vp_com->ref_count ++;
res = VP_COM_OK;
}
else
{
#ifdef USE_WIFI
if(vp_com->type == VP_COM_WIFI)
{
vp_com->init = (VP_COM_x_init) vp_com_wf_init;
vp_com->shutdown = (VP_COM_x_shutdown) vp_com_wf_shutdown;
vp_com->network_adapter_lookup = (VP_COM_x_network_adapter_lookup) vp_com_wf_network_adapter_lookup;
vp_com->local_config = (VP_COM_x_local_config) vp_com_wf_local_config;
vp_com->inquire = (VP_COM_x_inquire) vp_com_wf_inquire;
vp_com->connect = (VP_COM_x_connect) vp_com_wf_connect;
vp_com->disconnect = (VP_COM_x_disconnect) vp_com_wf_disconnect;
vp_com->get_rssi = (VP_COM_x_get_rssi) vp_com_wf_get_rssi;
vp_com->wait_connections = (VP_COM_x_wait_connections) vp_com_wf_wait_connections;
vp_com->open = (VP_COM_x_open) vp_com_wf_open;
vp_com->close = (VP_COM_x_close) vp_com_wf_close;
}
#endif // > USE_WIFI
#ifdef USE_SERIAL
if(vp_com->type == VP_COM_SERIAL)
{
vp_com->init = (VP_COM_x_init) vp_com_serial_init;
vp_com->shutdown = (VP_COM_x_shutdown) vp_com_serial_shutdown;
vp_com->network_adapter_lookup = (VP_COM_x_network_adapter_lookup) vp_com_serial_network_adapter_lookup;
vp_com->local_config = (VP_COM_x_local_config) vp_com_serial_local_config;
vp_com->inquire = (VP_COM_x_inquire) vp_com_serial_inquire;
vp_com->connect = (VP_COM_x_connect) vp_com_serial_connect;
vp_com->disconnect = (VP_COM_x_disconnect) vp_com_serial_disconnect;
vp_com->get_rssi = NULL;
vp_com->wait_connections = (VP_COM_x_wait_connections) vp_com_serial_wait_connections;
vp_com->open = (VP_COM_x_open) vp_com_serial_open;
vp_com->close = (VP_COM_x_close) vp_com_serial_close;
}
#endif
#ifdef USE_WIRED
if( vp_com->type == VP_COM_WIRED )
{
vp_com->init = (VP_COM_x_init) vp_com_wired_init;
vp_com->shutdown = (VP_COM_x_shutdown) vp_com_wired_shutdown;
vp_com->network_adapter_lookup = (VP_COM_x_network_adapter_lookup) vp_com_wired_network_adapter_lookup;
vp_com->local_config = (VP_COM_x_local_config) vp_com_wired_local_config;
vp_com->inquire = (VP_COM_x_inquire) vp_com_wired_inquire;
vp_com->connect = (VP_COM_x_connect) vp_com_wired_connect;
vp_com->disconnect = (VP_COM_x_disconnect) vp_com_wired_disconnect;
vp_com->get_rssi = NULL;
vp_com->wait_connections = (VP_COM_x_wait_connections) vp_com_wired_wait_connections;
vp_com->open = (VP_COM_x_open) vp_com_wired_open;
vp_com->close = (VP_COM_x_close) vp_com_wired_close;
}
#endif
if(VP_COM_INIT)
res = VP_COM_INIT();
if(res == VP_COM_OK)
{
vp_os_install_error_handler( VP_COM_SDK_SIGNATURE, vp_com_formatMessage );
vp_com->ref_count ++;
}
}
vp_os_mutex_unlock( &vp_com->mutex );
return res;
}
C_RESULT vp_com_wait_connections(vp_com_t* vp_com, vp_com_socket_t* server, vp_com_socket_t* client, int32_t queueLength)
{
VP_OS_ASSERT( vp_com != NULL );
return VP_COM_WAITCONNECTIONS( &vp_com->connection, server, client, queueLength );
}
C_RESULT vp_com_inquire(vp_com_t* vp_com, const char* deviceName, vp_com_inquiry_t callback, uint32_t timeout)
{
VP_OS_ASSERT( vp_com != NULL );
return VP_COM_INQUIRE( deviceName, callback, timeout );
}
