int ARSTREAM_ReaderTb_StartStreamTest (ARNETWORK_Manager_t *manager, const char *outPath)
{
int retVal = 0;
uint8_t *firstFrame;
uint32_t firstFrameSize;
eARSTREAM_ERROR err;
if (NULL != outPath)
{
outFile = fopen (outPath, "wb");
}
else
{
outFile = NULL;
}
ARSTREAM_ReaderTb_initMultiBuffers (FRAME_MAX_SIZE);
ARSAL_Sem_Init (&closeSem, 0, 0);
firstFrame = ARSTREAM_ReaderTb_GetNextFreeBuffer (&firstFrameSize, 0);
g_Reader = ARSTREAM_Reader_New (manager, DATA_BUFFER_ID, ACK_BUFFER_ID, ARSTREAM_ReaderTb_FrameCompleteCallback, firstFrame, firstFrameSize, ARSTREAM_TB_FRAG_SIZE, 0, NULL, &err);
if (g_Reader == NULL)
{
ARSAL_PRINT (ARSAL_PRINT_ERROR, __TAG__, "Error during ARSTREAM_Reader_New call : %s", ARSTREAM_Error_ToString(err));
return 1;
}
pthread_t streamsend, streamread;
pthread_create (&streamsend, NULL, ARSTREAM_Reader_RunDataThread, g_Reader);
pthread_create (&streamread, NULL, ARSTREAM_Reader_RunAckThread, g_Reader);
/* USER CODE */
running = 1;
ARSAL_Sem_Wait (&closeSem);
running = 0;
/* END OF USER CODE */
ARSTREAM_Reader_StopReader (g_Reader);
pthread_join (streamread, NULL);
pthread_join (streamsend, NULL);
ARSTREAM_Reader_Delete (&g_Reader);
ARSAL_Sem_Destroy (&closeSem);
return retVal;
}
void ARDISCOVERY_Connection_Device_StopListening (ARDISCOVERY_Connection_ConnectionData_t *connectionData)
{
eARDISCOVERY_ERROR error = ARDISCOVERY_OK;
/* Check parameter */
if (connectionData == NULL)
{
error = ARDISCOVERY_ERROR_BAD_PARAMETER;
}
if (error == ARDISCOVERY_OK)
{
/* Stop reception */
connectionData->isAlive = 0;
ARDISCOVERY_Connection_Unlock (connectionData);
/* wait the end of the run*/
ARSAL_Sem_Wait (&(connectionData->runningSem));
/* reset the runningSem */
ARSAL_Sem_Post(&(connectionData->runningSem));
}
}
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;
}
