// \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 ); }