diff --git a/SDK/BebopSample.c b/SDK/BebopSample.c new file mode 100755 index 0000000..b48be89 --- /dev/null +++ b/SDK/BebopSample.c @@ -0,0 +1,729 @@ +/* + Copyright (C) 2014 Parrot SA + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the + distribution. + * Neither the name of Parrot nor the names + of its contributors may be used to endorse or promote products + derived from this software without specific prior written + permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT + OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + SUCH DAMAGE. +*/ +/** + * @file BebopSample.c + * @brief This file contains sources about basic arsdk example sending commands to a bebop drone to pilot it, + * receive its battery level and display the video stream. + * @date 15/01/2015 + */ + +/***************************************** + * + * include file : + * + *****************************************/ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "BebopSample.h" +#include "ihm.h" + +/***************************************** + * + * define : + * + *****************************************/ +#define TAG "BebopSample" + +#define ERROR_STR_LENGTH 2048 + +#define BEBOP_IP_ADDRESS "192.168.42.1" +#define BEBOP_DISCOVERY_PORT 44444 + +#define DISPLAY_WITH_MPLAYER 1 + +#define FIFO_DIR_PATTERN "/tmp/arsdk_XXXXXX" +#define FIFO_NAME "arsdk_fifo" + +#define IHM +/***************************************** + * + * private header: + * + ****************************************/ + + +/***************************************** + * + * implementation : + * + *****************************************/ + +static char fifo_dir[] = FIFO_DIR_PATTERN; +static char fifo_name[128] = ""; + +int gIHMRun = 1; +char gErrorStr[ERROR_STR_LENGTH]; +IHM_t *ihm = NULL; + +FILE *videoOut = NULL; +int frameNb = 0; +ARSAL_Sem_t stateSem; +int isBebop2 = 0; + +static void signal_handler(int signal) +{ + gIHMRun = 0; +} + +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; + + + /* Set signal handlers */ + struct sigaction sig_action = { + .sa_handler = signal_handler, + }; + + int ret = sigaction(SIGINT, &sig_action, NULL); + if (ret < 0) + { + ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Unable to set SIGINT handler : %d(%s)", + errno, strerror(errno)); + return 1; + } + ret = sigaction(SIGPIPE, &sig_action, NULL); + if (ret < 0) + { + ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Unable to set SIGPIPE handler : %d(%s)", + errno, strerror(errno)); + return 1; + } + + + if (mkdtemp(fifo_dir) == NULL) + { + ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Mkdtemp failed."); + return 1; + } + snprintf(fifo_name, sizeof(fifo_name), "%s/%s", fifo_dir, FIFO_NAME); + + if(mkfifo(fifo_name, 0666) < 0) + { + ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Mkfifo failed: %d, %s", errno, strerror(errno)); + return 1; + } + + 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 Sample --"); + } + else + { + ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "-- Bebop Sample --"); + } + + if (!failed) + { + if (DISPLAY_WITH_MPLAYER) + { + // fork the process to launch mplayer + if ((child = fork()) == 0) + { + execlp("xterm", "xterm", "-e", "mplayer", "-demuxer", "h264es", fifo_name, "-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(fifo_name, "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 Sample --"); + } + else + { + IHM_PrintHeader (ihm, "-- Bebop Sample --"); + } + } + 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: %s", 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 + int i = 20; + ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- sleep 20 ... "); + while (gIHMRun && i--) + sleep(1); +#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)); + + unlink(fifo_name); + rmdir(fifo_dir); + + ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "-- END --"); + + return EXIT_SUCCESS; +} + +/***************************************** + * + * private implementation: + * + ****************************************/ + +// called when the state of the device controller has changed +void stateChanged (eARCONTROLLER_DEVICE_STATE newState, eARCONTROLLER_ERROR error, void *customData) +{ + ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - stateChanged newState: %d .....", newState); + + switch (newState) + { + case ARCONTROLLER_DEVICE_STATE_STOPPED: + ARSAL_Sem_Post (&(stateSem)); + //stop + gIHMRun = 0; + + break; + + case ARCONTROLLER_DEVICE_STATE_RUNNING: + ARSAL_Sem_Post (&(stateSem)); + break; + + default: + break; + } +} + +static void cmdBatteryStateChangedRcv(ARCONTROLLER_Device_t *deviceController, ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary) +{ + ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL; + ARCONTROLLER_DICTIONARY_ELEMENT_t *singleElement = NULL; + + if (elementDictionary == NULL) { + ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "elements is NULL"); + return; + } + + // get the command received in the device controller + HASH_FIND_STR (elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, singleElement); + + if (singleElement == NULL) { + ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "singleElement is NULL"); + return; + } + + // get the value + HASH_FIND_STR (singleElement->arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_BATTERYSTATECHANGED_PERCENT, arg); + + if (arg == NULL) { + ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "arg is NULL"); + return; + } + + // update UI + batteryStateChanged(arg->value.U8); +} + +static void cmdSensorStateListChangedRcv(ARCONTROLLER_Device_t *deviceController, ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary) +{ + FILE* fichier = NULL; + + + fichier = fopen("test.txt", "a"); + ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL; + ARCONTROLLER_DICTIONARY_ELEMENT_t *dictElement = NULL; + ARCONTROLLER_DICTIONARY_ELEMENT_t *dictTmp = NULL; + + eARCOMMANDS_COMMON_COMMONSTATE_SENSORSSTATESLISTCHANGED_SENSORNAME sensorName = ARCOMMANDS_COMMON_COMMONSTATE_SENSORSSTATESLISTCHANGED_SENSORNAME_MAX; + int sensorState = 0; + + if (elementDictionary == NULL) { + ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "elements is NULL"); + return; + } + + // get the command received in the device controller + HASH_ITER(hh, elementDictionary, dictElement, dictTmp) { + // get the Name + HASH_FIND_STR (dictElement->arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_SENSORSSTATESLISTCHANGED_SENSORNAME, arg); + if (arg != NULL) { + ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "arg sensorName is NULL"); + continue; + } + + sensorName = arg->value.I32; + + // get the state + HASH_FIND_STR (dictElement->arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_SENSORSSTATESLISTCHANGED_SENSORSTATE, arg); + if (arg == NULL) { + ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "arg sensorState is NULL"); + continue; + } + + sensorState = arg->value.U8; + ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "sensorName %d ; sensorState: %d", sensorName, sensorState); + fprintf(fichier, "sensorName %d ; sensorState: %d", sensorName, sensorState); + printf("sensorName %d ; sensorState: %d", sensorName, sensorState); + //sensorStateChanged(arg->value.I32, arg->value.U8); + } + sensorStateChanged(arg->value.I32, arg->value.U8); +} + +// called when a command has been received from the drone +void commandReceived (eARCONTROLLER_DICTIONARY_KEY commandKey, ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, void *customData) +{ + ARCONTROLLER_Device_t *deviceController = customData; + + if (deviceController == NULL) + return; + + // if the command received is a battery state changed + switch(commandKey) { + case ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_BATTERYSTATECHANGED: + cmdBatteryStateChangedRcv(deviceController, elementDictionary); + break; + case ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_SENSORSSTATESLISTCHANGED: + cmdSensorStateListChangedRcv(deviceController, elementDictionary); + break; + default: + break; + } +} + +void batteryStateChanged (uint8_t percent) +{ + // callback of changing of battery level + + if (ihm != NULL) + { + IHM_PrintBattery (ihm, percent); + } +} + +void sensorStateChanged (uint8_t sensorName, uint8_t sensorValue) +{ + // callback of changing of battery level + + if (ihm != NULL) + { + IHM_PrintSensors(ihm, sensorName, sensorValue); + } +} + + +eARCONTROLLER_ERROR decoderConfigCallback (ARCONTROLLER_Stream_Codec_t codec, void *customData) +{ + if (videoOut != NULL) + { + if (codec.type == ARCONTROLLER_STREAM_CODEC_TYPE_H264) + { + if (DISPLAY_WITH_MPLAYER) + { + fwrite(codec.parameters.h264parameters.spsBuffer, codec.parameters.h264parameters.spsSize, 1, videoOut); + fwrite(codec.parameters.h264parameters.ppsBuffer, codec.parameters.h264parameters.ppsSize, 1, videoOut); + + fflush (videoOut); + } + } + + } + else + { + ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG, "videoOut is NULL."); + } + + return ARCONTROLLER_OK; +} + + +eARCONTROLLER_ERROR didReceiveFrameCallback (ARCONTROLLER_Frame_t *frame, void *customData) +{ + if (videoOut != NULL) + { + if (frame != NULL) + { + if (DISPLAY_WITH_MPLAYER) + { + fwrite(frame->data, frame->used, 1, videoOut); + + fflush (videoOut); + } + } + else + { + ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG, "frame is NULL."); + } + } + else + { + ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG, "videoOut is NULL."); + } + + return ARCONTROLLER_OK; +} + + +// IHM callbacks: + +void onInputEvent (eIHM_INPUT_EVENT event, void *customData) +{ + // Manage IHM input events + ARCONTROLLER_Device_t *deviceController = (ARCONTROLLER_Device_t *)customData; + eARCONTROLLER_ERROR error = ARCONTROLLER_OK; + + switch (event) + { + case IHM_INPUT_EVENT_EXIT: + IHM_PrintInfo(ihm, "IHM_INPUT_EVENT_EXIT ..."); + gIHMRun = 0; + break; + case IHM_INPUT_EVENT_EMERGENCY: + if(deviceController != NULL) + { + // send a Emergency command to the drone + error = deviceController->aRDrone3->sendPilotingEmergency(deviceController->aRDrone3); + } + break; + case IHM_INPUT_EVENT_LAND: + if(deviceController != NULL) + { + // send a takeoff command to the drone + error = deviceController->aRDrone3->sendPilotingLanding(deviceController->aRDrone3); + } + break; + case IHM_INPUT_EVENT_TAKEOFF: + if(deviceController != NULL) + { + // send a landing command to the drone + error = deviceController->aRDrone3->sendPilotingTakeOff(deviceController->aRDrone3); + } + break; + case IHM_INPUT_EVENT_UP: + if(deviceController != NULL) + { + // set the flag and speed value of the piloting command + error = deviceController->aRDrone3->setPilotingPCMDGaz(deviceController->aRDrone3, 50); + } + break; + case IHM_INPUT_EVENT_DOWN: + if(deviceController != NULL) + { + error = deviceController->aRDrone3->setPilotingPCMDGaz(deviceController->aRDrone3, -50); + } + break; + case IHM_INPUT_EVENT_RIGHT: + if(deviceController != NULL) + { + error = deviceController->aRDrone3->setPilotingPCMDYaw(deviceController->aRDrone3, 50); + } + break; + case IHM_INPUT_EVENT_LEFT: + if(deviceController != NULL) + { + error = deviceController->aRDrone3->setPilotingPCMDYaw(deviceController->aRDrone3, -50); + } + break; + case IHM_INPUT_EVENT_FORWARD: + if(deviceController != NULL) + { + error = deviceController->aRDrone3->setPilotingPCMDPitch(deviceController->aRDrone3, 50); + error = deviceController->aRDrone3->setPilotingPCMDFlag(deviceController->aRDrone3, 1); + } + break; + case IHM_INPUT_EVENT_BACK: + if(deviceController != NULL) + { + error = deviceController->aRDrone3->setPilotingPCMDPitch(deviceController->aRDrone3, -50); + error = deviceController->aRDrone3->setPilotingPCMDFlag(deviceController->aRDrone3, 1); + } + break; + case IHM_INPUT_EVENT_ROLL_LEFT: + if(deviceController != NULL) + { + error = deviceController->aRDrone3->setPilotingPCMDRoll(deviceController->aRDrone3, -50); + error = deviceController->aRDrone3->setPilotingPCMDFlag(deviceController->aRDrone3, 1); + } + break; + case IHM_INPUT_EVENT_ROLL_RIGHT: + if(deviceController != NULL) + { + error = deviceController->aRDrone3->setPilotingPCMDRoll(deviceController->aRDrone3, 50); + error = deviceController->aRDrone3->setPilotingPCMDFlag(deviceController->aRDrone3, 1); + } + break; + case IHM_INPUT_EVENT_NONE: + if(deviceController != NULL) + { + error = deviceController->aRDrone3->setPilotingPCMD(deviceController->aRDrone3, 0, 0, 0, 0, 0, 0); + } + break; + default: + break; + } + + // This should be improved, here it just displays that one error occured + if (error != ARCONTROLLER_OK) + { + IHM_PrintInfo(ihm, "Error sending an event"); + } +} + +int customPrintCallback (eARSAL_PRINT_LEVEL level, const char *tag, const char *format, va_list va) +{ + // Custom callback used when ncurses is runing for not disturb the IHM + + if ((level == ARSAL_PRINT_ERROR) && (strcmp(TAG, tag) == 0)) + { + // Save the last Error + vsnprintf(gErrorStr, (ERROR_STR_LENGTH - 1), format, va); + gErrorStr[ERROR_STR_LENGTH - 1] = '\0'; + } + + return 1; +} diff --git a/SDK/BebopSample.h b/SDK/BebopSample.h new file mode 100755 index 0000000..ca304ba --- /dev/null +++ b/SDK/BebopSample.h @@ -0,0 +1,57 @@ +/* + Copyright (C) 2014 Parrot SA + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the + distribution. + * Neither the name of Parrot nor the names + of its contributors may be used to endorse or promote products + derived from this software without specific prior written + permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT + OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + SUCH DAMAGE. +*/ + +#ifndef _BEBOP_SAMPLE_H_ +#define _BEBOP_SAMPLE_H_ + +#include + +// called when the state of the device controller has changed +void stateChanged (eARCONTROLLER_DEVICE_STATE newState, eARCONTROLLER_ERROR error, void *customData); + +// called when a command has been received from the drone +void commandReceived (eARCONTROLLER_DICTIONARY_KEY commandKey, ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, void *customData); + +// IHM updates from commands +void batteryStateChanged (uint8_t percent); +void sensorStateChanged (uint8_t sensorName, uint8_t sensorValue); + +// called when a streaming frame has been received +eARCONTROLLER_ERROR didReceiveFrameCallback (ARCONTROLLER_Frame_t *frame, void *customData); + +eARCONTROLLER_ERROR decoderConfigCallback (ARCONTROLLER_Stream_Codec_t codec, void *customData); + +/* IHM callbacks: */ +void onInputEvent (eIHM_INPUT_EVENT event, void *customData); + +int customPrintCallback (eARSAL_PRINT_LEVEL level, const char *tag, const char *format, va_list va); + +#endif /* BEBOP_SAMPLE_H */ diff --git a/SDK/ihm.c b/SDK/ihm.c new file mode 100755 index 0000000..354902a --- /dev/null +++ b/SDK/ihm.c @@ -0,0 +1,331 @@ +/* + Copyright (C) 2014 Parrot SA + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the + distribution. + * Neither the name of Parrot nor the names + of its contributors may be used to endorse or promote products + derived from this software without specific prior written + permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT + OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + SUCH DAMAGE. +*/ +/** + * @file ihm.c + * @brief This file contains sources about ncurses IHM used by arsdk example "BebopSample" + * @date 15/01/2015 + */ + +/***************************************** + * + * include file : + * + *****************************************/ + +#include +#include +#include + +#include + +#include "ihm.h" + +/***************************************** + * + * define : + * + *****************************************/ + +#define HEADER_X 0 +#define HEADER_Y 0 + +#define INFO_X 0 +#define INFO_Y 2 + +#define BATTERY_X 0 +#define BATTERY_Y 4 + +#define SENSOR_X 0 +#define SENSOR_Y 6 + +#define TEST_X 0 +#define TEST_Y 8 +/***************************************** + * + * private header: + * + ****************************************/ +void *IHM_InputProcessing(void *data); + +/***************************************** + * + * implementation : + * + *****************************************/ + +IHM_t *IHM_New (IHM_onInputEvent_t onInputEventCallback) +{ + int failed = 0; + IHM_t *newIHM = NULL; + + // check parameters + if (onInputEventCallback == NULL) + { + failed = 1; + } + + if (!failed) + { + // Initialize IHM + newIHM = malloc(sizeof(IHM_t)); + if (newIHM != NULL) + { + // Initialize ncurses + newIHM->mainWindow = initscr(); + newIHM->inputThread = NULL; + newIHM->run = 1; + newIHM->onInputEventCallback = onInputEventCallback; + newIHM->customData = NULL; + } + else + { + failed = 1; + } + } + + if (!failed) + { + raw(); // Line buffering disabled + keypad(stdscr, TRUE); + noecho(); // Don't echo() while we do getch + timeout(100); + + refresh(); + } + + if (!failed) + { + //start input thread + if(ARSAL_Thread_Create(&(newIHM->inputThread), IHM_InputProcessing, newIHM) != 0) + { + failed = 1; + } + } + + if (failed) + { + IHM_Delete (&newIHM); + } + + return newIHM; +} + +void IHM_Delete (IHM_t **ihm) +{ + // Clean up + + if (ihm != NULL) + { + if ((*ihm) != NULL) + { + (*ihm)->run = 0; + + if ((*ihm)->inputThread != NULL) + { + ARSAL_Thread_Join((*ihm)->inputThread, NULL); + ARSAL_Thread_Destroy(&((*ihm)->inputThread)); + (*ihm)->inputThread = NULL; + } + + delwin((*ihm)->mainWindow); + (*ihm)->mainWindow = NULL; + endwin(); + refresh(); + + free (*ihm); + (*ihm) = NULL; + } + } +} + +void IHM_setCustomData(IHM_t *ihm, void *customData) +{ + if (ihm != NULL) + { + ihm->customData = customData; + } +} + +void *IHM_InputProcessing(void *data) +{ + IHM_t *ihm = (IHM_t *) data; + int key = 0; + + if (ihm != NULL) + { + while (ihm->run) + { + key = getch(); + + if ((key == 27) || (key =='q')) + { + if(ihm->onInputEventCallback != NULL) + { + ihm->onInputEventCallback (IHM_INPUT_EVENT_EXIT, ihm->customData); + } + } + else if(key == KEY_UP) + { + if(ihm->onInputEventCallback != NULL) + { + ihm->onInputEventCallback (IHM_INPUT_EVENT_UP, ihm->customData); + } + } + else if(key == KEY_DOWN) + { + if(ihm->onInputEventCallback != NULL) + { + ihm->onInputEventCallback (IHM_INPUT_EVENT_DOWN, ihm->customData); + } + } + else if(key == KEY_LEFT) + { + if(ihm->onInputEventCallback != NULL) + { + ihm->onInputEventCallback (IHM_INPUT_EVENT_LEFT, ihm->customData); + } + } + else if(key == KEY_RIGHT) + { + if(ihm->onInputEventCallback != NULL) + { + ihm->onInputEventCallback (IHM_INPUT_EVENT_RIGHT, ihm->customData); + } + } + else if(key == 'e') + { + if(ihm->onInputEventCallback != NULL) + { + ihm->onInputEventCallback (IHM_INPUT_EVENT_EMERGENCY, ihm->customData); + } + } + else if(key == 't') + { + if(ihm->onInputEventCallback != NULL) + { + ihm->onInputEventCallback (IHM_INPUT_EVENT_TAKEOFF, ihm->customData); + } + } + else if(key == ' ') + { + if(ihm->onInputEventCallback != NULL) + { + ihm->onInputEventCallback (IHM_INPUT_EVENT_LAND, ihm->customData); + } + } + else if(key == 'r') + { + if(ihm->onInputEventCallback != NULL) + { + ihm->onInputEventCallback (IHM_INPUT_EVENT_FORWARD, ihm->customData); + } + } + else if(key == 'f') + { + if(ihm->onInputEventCallback != NULL) + { + ihm->onInputEventCallback (IHM_INPUT_EVENT_BACK, ihm->customData); + } + } + else if(key == 'd') + { + if(ihm->onInputEventCallback != NULL) + { + ihm->onInputEventCallback (IHM_INPUT_EVENT_ROLL_LEFT, ihm->customData); + } + } + else if(key == 'g') + { + if(ihm->onInputEventCallback != NULL) + { + ihm->onInputEventCallback (IHM_INPUT_EVENT_ROLL_RIGHT, ihm->customData); + } + } + else + { + if(ihm->onInputEventCallback != NULL) + { + ihm->onInputEventCallback (IHM_INPUT_EVENT_NONE, ihm->customData); + } + } + + usleep(10); + } + } + + return NULL; +} + +void IHM_PrintHeader(IHM_t *ihm, char *headerStr) +{ + if (ihm != NULL) + { + move(HEADER_Y, 0); // move to begining of line + clrtoeol(); // clear line + mvprintw(HEADER_Y, HEADER_X, headerStr); + } +} + +void IHM_PrintInfo(IHM_t *ihm, char *infoStr) +{ + if (ihm != NULL) + { + move(INFO_Y, 0); // move to begining of line + clrtoeol(); // clear line + mvprintw(INFO_Y, INFO_X, infoStr); + } +} + +void IHM_PrintBattery(IHM_t *ihm, uint8_t percent) +{ + if (ihm != NULL) + { + move(BATTERY_Y, 0); // move to begining of line + clrtoeol(); // clear line + mvprintw(BATTERY_Y, BATTERY_X, "Battery: %d", percent); + + } +} + +void IHM_PrintSensors(IHM_t *ihm, uint8_t sensorName, uint8_t sensorValue){ + if (ihm != NULL) + { + move(TEST_Y, 0); + clrtoeol(); + mvprintw(TEST_Y, TEST_X, "Test"); + move(SENSOR_Y, 0); // move to begining of line + clrtoeol(); // clear line + mvprintw(SENSOR_Y, SENSOR_X, "Sensor: %d, Value: %d", sensorName, sensorValue); + } +} + + + diff --git a/SDK/ihm.h b/SDK/ihm.h new file mode 100755 index 0000000..8ba1ba6 --- /dev/null +++ b/SDK/ihm.h @@ -0,0 +1,76 @@ +/* + Copyright (C) 2014 Parrot SA + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the + distribution. + * Neither the name of Parrot nor the names + of its contributors may be used to endorse or promote products + derived from this software without specific prior written + permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT + OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + SUCH DAMAGE. +*/ + +#ifndef _BEBOP_SAMPLE_IHM_H_ +#define _BEBOP_SAMPLE_IHM_H_ + +#include +#include + +typedef enum +{ + IHM_INPUT_EVENT_NONE, + IHM_INPUT_EVENT_EXIT, + IHM_INPUT_EVENT_EMERGENCY, + IHM_INPUT_EVENT_TAKEOFF, + IHM_INPUT_EVENT_LAND, + IHM_INPUT_EVENT_UP, + IHM_INPUT_EVENT_DOWN, + IHM_INPUT_EVENT_RIGHT, + IHM_INPUT_EVENT_LEFT, + IHM_INPUT_EVENT_FORWARD, + IHM_INPUT_EVENT_BACK, + IHM_INPUT_EVENT_ROLL_LEFT, + IHM_INPUT_EVENT_ROLL_RIGHT, +}eIHM_INPUT_EVENT; + +typedef void (*IHM_onInputEvent_t) (eIHM_INPUT_EVENT event, void *customData); + +typedef struct +{ + WINDOW *mainWindow; + ARSAL_Thread_t inputThread; + int run; + IHM_onInputEvent_t onInputEventCallback; + void *customData; +}IHM_t; + +IHM_t *IHM_New (IHM_onInputEvent_t onInputEventCallback); +void IHM_Delete (IHM_t **ihm); + +void IHM_setCustomData(IHM_t *ihm, void *customData); + +void IHM_PrintHeader(IHM_t *ihm, char *headerStr); +void IHM_PrintInfo(IHM_t *ihm, char *infoStr); +void IHM_PrintBattery(IHM_t *ihm, uint8_t percent); +void IHM_PrintSensors(IHM_t *ihm, uint8_t sensorName, uint8_t sensorValue); + +#endif /* _BEBOP_SAMPLE_IHM_H_ */ -- libgit2 0.21.2