void ARDISCOVERY_AvahiDiscovery_Publish(ARDISCOVERY_AvahiDiscovery_PublisherData_t* serviceData) { eARDISCOVERY_ERROR error = ARDISCOVERY_OK; if (serviceData == NULL) { ERR("Null parameter"); error = ARDISCOVERY_ERROR; } if (error == ARDISCOVERY_OK) { /* Build service name */ serviceData->serviceName = ARDISCOVERY_AvahiDiscovery_BuildName(); if (serviceData->serviceName == NULL) { error = ARDISCOVERY_ERROR_BUILD_NAME; } } error = ARDISCOVERY_AvahiDiscovery_CreateService(serviceData); if (error != ARDISCOVERY_OK) { ERR("error: %s", ARDISCOVERY_Error_ToString (error)); } }
ARDISCOVERY_AvahiDiscovery_PublisherData_t* ARDISCOVERY_AvahiDiscovery_Publisher_New(char *serviceName, char *serviceType, uint32_t publishedPort, eARDISCOVERY_ERROR *errorPtr) { /* * Create and initialize discovery data */ ARDISCOVERY_AvahiDiscovery_PublisherData_t *serviceData = NULL; eARDISCOVERY_ERROR error = ARDISCOVERY_OK; if (serviceName == NULL || serviceType == NULL) { ERR("Null parameter"); error = ARDISCOVERY_ERROR; } if (error == ARDISCOVERY_OK) { serviceData = malloc(sizeof(ARDISCOVERY_AvahiDiscovery_PublisherData_t)); if (serviceData != NULL) { /* Init Avahi data */ serviceData->devicePort = publishedPort; /* Set Service Type */ if (error == ARDISCOVERY_OK) { serviceData->serviceType = malloc(sizeof(uint8_t) * ARDISCOVERY_AVAHIDISCOVERY_SERVICETYPE_SIZE); if (serviceData->serviceType != NULL) { strcpy(serviceData->serviceType, serviceType); } else { error = ARDISCOVERY_ERROR_ALLOC; } } /* Set Service Name */ if (error == ARDISCOVERY_OK) { serviceData->serviceName = malloc(sizeof(uint8_t) * ARDISCOVERY_AVAHIDISCOVERY_SERVICENAME_SIZE); if (serviceData->serviceName != NULL) { strcpy(serviceData->serviceName, serviceName); } else { error = ARDISCOVERY_ERROR_ALLOC; } } } } /* Delete connection data if an error occurred */ if (error != ARDISCOVERY_OK) { ERR("error: %s", ARDISCOVERY_Error_ToString (error)); ARDISCOVERY_AvahiDiscovery_Publisher_Delete(&serviceData); } if (errorPtr != NULL) { *errorPtr = error; } return serviceData; }
eARDISCOVERY_ERROR ARDISCOVERY_Connection_DeviceListeningLoop (ARDISCOVERY_Connection_ConnectionData_t *connectionData, int port) { /* * Initialize connection */ eARDISCOVERY_ERROR error = ARDISCOVERY_OK; eARDISCOVERY_ERROR loopError = ARDISCOVERY_OK; int deviceSocket = -1; /* Check parameter */ if (connectionData == NULL) { error = ARDISCOVERY_ERROR_BAD_PARAMETER; } if (error == ARDISCOVERY_OK) { /* check if it is already running */ if (ARSAL_Sem_Trywait(&(connectionData->runningSem)) != 0) { error = ARDISCOVERY_ERROR_BUSY; } } if (error == ARDISCOVERY_OK) { /* Initialize the server socket */ error = ARDISCOVERY_Connection_DeviceInitSocket (&deviceSocket, port); } if (error == ARDISCOVERY_OK) { /* Initialize is alive */ connectionData->isAlive = 1; /* while is alive */ while (connectionData->isAlive == 1) { /* Wait for any incoming connection from controller */ loopError = ARDISCOVERY_Connection_DeviceAccept (connectionData, deviceSocket); if (loopError == ARDISCOVERY_OK) { ARSAL_PRINT(ARSAL_PRINT_DEBUG, ARDISCOVERY_CONNECTION_TAG, "Device accepts a socket"); /* receiption */ loopError = ARDISCOVERY_Connection_RxPending (connectionData); } if (loopError == ARDISCOVERY_OK) { /* sending */ loopError = ARDISCOVERY_Connection_TxPending (connectionData); } /* close the client socket */ if (connectionData->socket != -1) { ARSAL_Socket_Close (connectionData->socket); connectionData->socket = -1; } if (loopError != ARDISCOVERY_OK) { /* print the error occurred */ ARSAL_PRINT(ARSAL_PRINT_ERROR, ARDISCOVERY_CONNECTION_TAG, "error: %s", ARDISCOVERY_Error_ToString (loopError)); } } /* close deviceSocket */ if(deviceSocket != -1) { ARSAL_Socket_Close (deviceSocket); deviceSocket = -1; } /* reset the runningSem */ ARSAL_Sem_Post(&(connectionData->runningSem)); } return error; }
int main (int argc, char *argv[]) { // local declarations int failed = 0; ARDISCOVERY_Device_t *device = NULL; ARCONTROLLER_Device_t *deviceController = NULL; eARCONTROLLER_ERROR error = ARCONTROLLER_OK; eARCONTROLLER_DEVICE_STATE deviceState = ARCONTROLLER_DEVICE_STATE_MAX; pid_t child = 0; ARSAL_Sem_Init (&(stateSem), 0, 0); ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Select your Bebop : Bebop (1) ; Bebop2 (2)"); char answer = '1'; scanf("%c", &answer); if (answer == '2') { isBebop2 = 1; } if(isBebop2) { ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "-- Bebop 2 Piloting --"); } else { ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "-- Bebop Piloting --"); } if (!failed) { if (DISPLAY_WITH_MPLAYER) { // fork the process to launch mplayer if ((child = fork()) == 0) { execlp("xterm", "xterm", "-e", "mplayer", "-demuxer", "h264es", "video_fifo.h264", "-benchmark", "-really-quiet", NULL); ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Missing mplayer, you will not see the video. Please install mplayer and xterm."); return -1; } } if (DISPLAY_WITH_MPLAYER) { videoOut = fopen("./video_fifo.h264", "w"); } } #ifdef IHM ihm = IHM_New (&onInputEvent); if (ihm != NULL) { gErrorStr[0] = '\0'; ARSAL_Print_SetCallback (customPrintCallback); //use a custom callback to print, for not disturb ncurses IHM if(isBebop2) { IHM_PrintHeader (ihm, "-- Bebop 2 Piloting --"); } else { IHM_PrintHeader (ihm, "-- Bebop Piloting --"); } } else { ARSAL_PRINT (ARSAL_PRINT_ERROR, TAG, "Creation of IHM failed."); failed = 1; } #endif // create a discovery device if (!failed) { ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- init discovey device ... "); eARDISCOVERY_ERROR errorDiscovery = ARDISCOVERY_OK; device = ARDISCOVERY_Device_New (&errorDiscovery); if (errorDiscovery == ARDISCOVERY_OK) { ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - ARDISCOVERY_Device_InitWifi ..."); // create a Bebop drone discovery device (ARDISCOVERY_PRODUCT_ARDRONE) if(isBebop2) { errorDiscovery = ARDISCOVERY_Device_InitWifi (device, ARDISCOVERY_PRODUCT_BEBOP_2, "bebop2", BEBOP_IP_ADDRESS, BEBOP_DISCOVERY_PORT); } else { errorDiscovery = ARDISCOVERY_Device_InitWifi (device, ARDISCOVERY_PRODUCT_ARDRONE, "bebop", BEBOP_IP_ADDRESS, BEBOP_DISCOVERY_PORT); } if (errorDiscovery != ARDISCOVERY_OK) { failed = 1; ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Discovery error :%s", ARDISCOVERY_Error_ToString(errorDiscovery)); } } else { ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Discovery error :%s", ARDISCOVERY_Error_ToString(errorDiscovery)); failed = 1; } } // create a device controller if (!failed) { deviceController = ARCONTROLLER_Device_New (device, &error); if (error != ARCONTROLLER_OK) { ARSAL_PRINT (ARSAL_PRINT_ERROR, TAG, "Creation of deviceController failed."); failed = 1; } else { IHM_setCustomData(ihm, deviceController); } } if (!failed) { ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- delete discovey device ... "); ARDISCOVERY_Device_Delete (&device); } // add the state change callback to be informed when the device controller starts, stops... if (!failed) { error = ARCONTROLLER_Device_AddStateChangedCallback (deviceController, stateChanged, deviceController); if (error != ARCONTROLLER_OK) { ARSAL_PRINT (ARSAL_PRINT_ERROR, TAG, "add State callback failed."); failed = 1; } } // add the command received callback to be informed when a command has been received from the device if (!failed) { error = ARCONTROLLER_Device_AddCommandReceivedCallback (deviceController, commandReceived, deviceController); if (error != ARCONTROLLER_OK) { ARSAL_PRINT (ARSAL_PRINT_ERROR, TAG, "add callback failed."); failed = 1; } } // add the frame received callback to be informed when a streaming frame has been received from the device if (!failed) { ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- set Video callback ... "); error = ARCONTROLLER_Device_SetVideoStreamCallbacks (deviceController, decoderConfigCallback, didReceiveFrameCallback, NULL , NULL); if (error != ARCONTROLLER_OK) { failed = 1; ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error :%", ARCONTROLLER_Error_ToString(error)); } } if (!failed) { IHM_PrintInfo(ihm, "Connecting ..."); ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Connecting ..."); error = ARCONTROLLER_Device_Start (deviceController); if (error != ARCONTROLLER_OK) { failed = 1; ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error :%s", ARCONTROLLER_Error_ToString(error)); } } if (!failed) { // wait state update update ARSAL_Sem_Wait (&(stateSem)); deviceState = ARCONTROLLER_Device_GetState (deviceController, &error); if ((error != ARCONTROLLER_OK) || (deviceState != ARCONTROLLER_DEVICE_STATE_RUNNING)) { failed = 1; ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- deviceState :%d", deviceState); ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error :%s", ARCONTROLLER_Error_ToString(error)); } } // send the command that tells to the Bebop to begin its streaming if (!failed) { ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- send StreamingVideoEnable ... "); error = deviceController->aRDrone3->sendMediaStreamingVideoEnable (deviceController->aRDrone3, 1); if (error != ARCONTROLLER_OK) { ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error :%s", ARCONTROLLER_Error_ToString(error)); failed = 1; } } if (!failed) { IHM_PrintInfo(ihm, "Running ... ('t' to takeoff ; Spacebar to land ; 'e' for emergency ; Arrow keys and ('r','f','d','g') to move ; 'q' to quit)"); #ifdef IHM while (gIHMRun) { usleep(50); } #else ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- sleep 20 ... "); sleep(20); #endif } #ifdef IHM IHM_Delete (&ihm); #endif // we are here because of a disconnection or user has quit IHM, so safely delete everything if (deviceController != NULL) { deviceState = ARCONTROLLER_Device_GetState (deviceController, &error); if ((error == ARCONTROLLER_OK) && (deviceState != ARCONTROLLER_DEVICE_STATE_STOPPED)) { IHM_PrintInfo(ihm, "Disconnecting ..."); ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Disconnecting ..."); error = ARCONTROLLER_Device_Stop (deviceController); if (error == ARCONTROLLER_OK) { // wait state update update ARSAL_Sem_Wait (&(stateSem)); } } IHM_PrintInfo(ihm, ""); ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "ARCONTROLLER_Device_Delete ..."); ARCONTROLLER_Device_Delete (&deviceController); if (DISPLAY_WITH_MPLAYER) { fflush (videoOut); fclose (videoOut); if (child > 0) { kill(child, SIGKILL); } } } ARSAL_Sem_Destroy (&(stateSem)); ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "-- END --"); return EXIT_SUCCESS; }
int ardiscoveryConnect (BD_MANAGER_t *deviceManager) { int failed = 0; ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- ARDiscovery Connection"); eARDISCOVERY_ERROR err = ARDISCOVERY_OK; printf("Je suis avant le ARDISCOVERY_CONNECTION\n"); ARDISCOVERY_Connection_ConnectionData_t *discoveryData = ARDISCOVERY_Connection_New (ARDISCOVERY_Connection_SendJsonCallback, ARDISCOVERY_Connection_ReceiveJsonCallback, deviceManager, &err); if (discoveryData == NULL || err != ARDISCOVERY_OK) { ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error while creating discoveryData : %s", ARDISCOVERY_Error_ToString(err)); failed = 1; } if (!failed) { eARDISCOVERY_ERROR err = ARDISCOVERY_Connection_ControllerConnection(discoveryData, BD_DISCOVERY_PORT, BD_IP_ADDRESS); printf("ARDISCOVERY_CONNECTION_CONTROLLERCONNECTION créé\n"); if (err != ARDISCOVERY_OK) { printf("Il y a une erreur\n"); ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error while opening discovery connection : %s", ARDISCOVERY_Error_ToString(err)); failed = 1; } } ARDISCOVERY_Connection_Delete(&discoveryData); return failed; }