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