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; }
ARCONTROLLER_Network_t *ARCONTROLLER_Network_New (ARDISCOVERY_Device_t *discoveryDevice, ARCONTROLLER_Network_DisconnectionCallback_t disconnectionCallback, ARDISCOVERY_Device_ConnectionJsonCallback_t sendJsonCallback, ARDISCOVERY_Device_ConnectionJsonCallback_t receiveJsonCallback, void *customData, eARCONTROLLER_ERROR *error) { // -- Create a new Network Controller -- //local declarations eARCONTROLLER_ERROR localError = ARCONTROLLER_OK; ARCONTROLLER_Network_t *networkController = NULL; eARDISCOVERY_ERROR dicoveryError = ARDISCOVERY_OK; // Check parameters if (discoveryDevice == NULL) { localError = ARCONTROLLER_ERROR_BAD_PARAMETER; } // No Else: the checking parameters sets localError to ARNETWORK_ERROR_BAD_PARAMETER and stop the processing if (localError == ARCONTROLLER_OK) { // Create the Network Controller networkController = malloc (sizeof (ARCONTROLLER_Network_t)); if (networkController != NULL) { // Initialize to default values networkController->discoveryDevice = NULL; networkController->networkALManager = NULL; networkController->networkManager = NULL; networkController->rxThread = NULL; networkController->txThread = NULL; networkController->readerThreads = NULL; networkController->readerThreadsData = NULL; networkController->state = ARCONTROLLER_NETWORK_STATE_RUNNING; //video part networkController->hasVideo = 0; networkController->videoController = NULL; //Connection callback networkController->sendJsonCallback = sendJsonCallback; networkController->receiveJsonCallback = receiveJsonCallback; networkController->disconnectionCallback = disconnectionCallback; networkController->callbacksCustomData = customData; // init networkConfiguration networkController->networkConfig.controllerToDeviceNotAckId = -1; networkController->networkConfig.controllerToDeviceAckId = -1; networkController->networkConfig.controllerToDeviceHightPriority = -1; networkController->networkConfig.controllerToDeviceARStreamAck = -1; networkController->networkConfig.deviceToControllerNotAckId = -1; networkController->networkConfig.deviceToControllerAckId = -1; //networkController->networkConfig.deviceToControllerHightPriority = -1; networkController->networkConfig.deviceToControllerARStreamData = -1; networkController->networkConfig.numberOfControllerToDeviceParam = 0; networkController->networkConfig.controllerToDeviceParams = NULL; networkController->networkConfig.numberOfDeviceToControllerParam = 0; networkController->networkConfig.deviceToControllerParams = NULL; networkController->networkConfig.pingDelayMs =-1; networkController->networkConfig.numberOfDeviceToControllerCommandsBufferIds = 0; networkController->networkConfig.deviceToControllerCommandsBufferIds = NULL; /* Create the mutex/condition */ if ( (localError == ARCONTROLLER_OK) && (ARSAL_Mutex_Init (&(networkController->mutex)) != 0)) { localError = ARCONTROLLER_ERROR_INIT_MUTEX; } } else { localError = ARCONTROLLER_ERROR_ALLOC; } } if (localError == ARCONTROLLER_OK) { // Copy the device eARDISCOVERY_ERROR dicoveryError = ARDISCOVERY_OK; networkController->discoveryDevice = ARDISCOVERY_Device_NewByCopy (discoveryDevice, &dicoveryError); if (dicoveryError != ARDISCOVERY_OK) { localError = ARCONTROLLER_ERROR_INIT_DEVICE_COPY; } } // Check if it is a wifi device if ((localError == ARCONTROLLER_OK) && (ARDISCOVERY_getProductService (networkController->discoveryDevice->productID) == ARDISCOVERY_PRODUCT_NSNETSERVICE)) { // Add callbacks for the connection json part dicoveryError = ARDISCOVERY_Device_WifiAddConnectionCallbacks (networkController->discoveryDevice, ARCONTROLLER_Network_OnSendJson, ARCONTROLLER_Network_OnReceiveJson, networkController); if (dicoveryError != ARDISCOVERY_OK) { localError = ARCONTROLLER_ERROR_INIT_DEVICE_JSON_CALLBACK; } } if (localError == ARCONTROLLER_OK) { // Initialize the network Configuration eARDISCOVERY_ERROR dicoveryError = ARDISCOVERY_OK; dicoveryError = ARDISCOVERY_Device_InitNetworkConfiguration (networkController->discoveryDevice, &(networkController->networkConfig)); if (dicoveryError != ARDISCOVERY_OK) { localError = ARCONTROLLER_ERROR_INIT_NETWORK_CONFIG; } } if (localError == ARCONTROLLER_OK) { // Check if the device has video if (networkController->networkConfig.deviceToControllerARStreamData != -1) { networkController->hasVideo = 1; networkController->videoController = ARCONTROLLER_Stream_New (&(networkController->networkConfig), networkController->discoveryDevice, &localError); } //NO else ; device has not video } // No else: skipped by an error if (localError == ARCONTROLLER_OK) { // Create the NetworkAL eARDISCOVERY_ERROR dicoveryError = ARDISCOVERY_OK; eARNETWORKAL_ERROR netALError = ARNETWORKAL_OK; networkController->networkALManager = ARDISCOVERY_Device_NewARNetworkAL (networkController->discoveryDevice, &dicoveryError, &netALError); if (dicoveryError != ARDISCOVERY_OK) { if (netALError != ARNETWORKAL_OK) { ARSAL_PRINT (ARSAL_PRINT_ERROR, ARCONTROLLER_NETWORK_TAG, "error: %s", ARNETWORKAL_Error_ToString (netALError)); } localError = ARCONTROLLER_ERROR_INIT_ARNETWORKAL_MANAGER; } } if (localError == ARCONTROLLER_OK) { // Create the ARNetworkManager. eARNETWORK_ERROR netError = ARNETWORK_OK; networkController->networkManager = ARNETWORK_Manager_New (networkController->networkALManager, networkController->networkConfig.numberOfControllerToDeviceParam, networkController->networkConfig.controllerToDeviceParams, networkController->networkConfig.numberOfDeviceToControllerParam, networkController->networkConfig.deviceToControllerParams, networkController->networkConfig.pingDelayMs, ARCONTROLLER_Network_OnDisconnectNetwork, networkController, &netError); if (netError != ARNETWORK_OK) { localError = ARCONTROLLER_ERROR_INIT_ARNETWORK_MANAGER; } } if (localError == ARCONTROLLER_OK) { // Create the Network receiver and transmitter Threads localError = ARCONTROLLER_Network_CreateNetworkThreads (networkController); } if (localError == ARCONTROLLER_OK) { // Create the reader Threads localError = ARCONTROLLER_Network_CreateReaderThreads (networkController); } // delete the Network Controller if an error occurred if (localError != ARCONTROLLER_OK) { ARSAL_PRINT (ARSAL_PRINT_ERROR, ARCONTROLLER_NETWORK_TAG, "error: %s", ARCONTROLLER_Error_ToString (localError)); ARCONTROLLER_Network_Delete (&networkController); } // No else: skipped by an error // Return the error if (error != NULL) { *error = localError; } // No else: error is not returned return networkController; }
void *ARCONTROLLER_Network_ReaderRun (void *data) { // -- Read and decode one input network buffer -- eARCONTROLLER_ERROR error = ARCONTROLLER_OK; ARCONTROLLER_Network_t *networkController = NULL; int readerBufferId = -1; // Allocate some space for incoming data. const size_t maxLength = 128 * 1024; void *readData = malloc (maxLength); if (readData == NULL) { error = ARCONTROLLER_ERROR_ALLOC; } if (error == ARCONTROLLER_OK) { if (data != NULL) { //get thread data networkController = ((ARCONTROLLER_NETWORK_THREAD_DATA_t *) data)->networkController; readerBufferId = ((ARCONTROLLER_NETWORK_THREAD_DATA_t *) data)->readerBufferId; if (networkController == NULL) { error = ARCONTROLLER_ERROR_BAD_PARAMETER; } } else { error = ARCONTROLLER_ERROR_BAD_PARAMETER; } } if (error == ARCONTROLLER_OK) { while (networkController->state != ARCONTROLLER_NETWORK_STATE_STOPPED) { if (networkController->state == ARCONTROLLER_NETWORK_STATE_RUNNING) { eARNETWORK_ERROR netError = ARNETWORK_OK; int length = 0; int skip = 0; //read data netError = ARNETWORK_Manager_ReadDataWithTimeout (networkController->networkManager, readerBufferId, readData, maxLength, &length, ARCONTROLLER_NETWORK_READING_TIMEOUT_MS); if (netError != ARNETWORK_OK) { if (netError != ARNETWORK_ERROR_BUFFER_EMPTY) { ARSAL_PRINT(ARSAL_PRINT_ERROR, ARCONTROLLER_NETWORK_TAG, "ARNETWORK_Manager_ReadDataWithTimeout () failed : %s", ARNETWORK_Error_ToString(netError)); } skip = 1; } if (!skip) { // Forward data to the CommandsManager eARCOMMANDS_DECODER_ERROR cmdError = ARCOMMANDS_DECODER_OK; cmdError = ARCOMMANDS_Decoder_DecodeBuffer ((uint8_t *)readData, length); if ((cmdError != ARCOMMANDS_DECODER_OK) && (cmdError != ARCOMMANDS_DECODER_ERROR_NO_CALLBACK)) { char msg[128]; ARCOMMANDS_Decoder_DescribeBuffer ((uint8_t *)readData, length, msg, sizeof(msg)); ARSAL_PRINT(ARSAL_PRINT_ERROR, ARCONTROLLER_NETWORK_TAG, "ARCOMMANDS_Decoder_DecodeBuffer () failed : %d %s", cmdError, msg); } } //NO ELSE ; no data read } else { //sleep sleep (1); //TODO !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! replace by signal } } } // Print Error if (error != ARCONTROLLER_OK) { ARSAL_PRINT(ARSAL_PRINT_ERROR, ARCONTROLLER_NETWORK_TAG, "error: %s", ARCONTROLLER_Error_ToString (error)); } // cleanup if (readData != NULL) { free (readData); readData = NULL; } return NULL; }
/*********************************** MAIN ****************************/ 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); #ifdef _ROLLINGSPIDER_PILOTING_IHM_H_ 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 IHM_PrintHeader(ihm, "-- Rolling Spider 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 discovery device ..."); eARDISCOVERY_ERROR errorDiscovery = ARDISCOVERY_OK; device = ARDISCOVERY_Device_New(&errorDiscovery); if (errorDiscovery == ARDISCOVERY_OK) { ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- ARDISCOVERY_Device_InitBLE ..."); // create a Rolling Spider discovery device (ARDISCOVERY_PRODUCT_JS) /**errorDiscovery = ARDISCOVERY_Device_InitBLE(device, ARDISCOVERY_PRODUCT_MINIDRONE,bleDeviceManager,BLEDevice);**/ } } // 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 commands received callback // 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 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; } } // connecting to the minidrone 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)); } } //waiting for a state update if (!failed) { 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)); } } //running step when the drone is ready to work if (!failed) { IHM_PrintInfo(ihm, "Running ... ('t' to takeoff; 'l' to land; 'e' for emergency; 'q' to quit)"); #ifdef _ROLLINGSPIDER_PILOTING_IHM_H_ while (gIHMRun) { usleep(50); } #else ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- sleep 20 ... "); sleep(20); ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- sleep end ... "); #endif } #ifdef _ROLLINGSPIDER_PILOTING_IHM_H_ 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); } ARSAL_Sem_Destroy(&(stateSem)); ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "-- END --"); return EXIT_SUCCESS; }