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;
}
Example #2
0
/*********************************** 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;
}