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;
}
/*********************************** 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;
}