/** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup FlightPlan Flight Plan Module * @brief Executes flight plan scripts in Python * @{ * * @file flightplan.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Executes flight plan scripts in Python * * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. * * You should have received a copy of the GNU General Public License along * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include #include "flightplan.h" #include "flightplanstatus.h" #include "flightplancontrol.h" #include "flightplansettings.h" #include "taskinfo.h" #include "pm.h" // Private constants #define STACK_SIZE_BYTES 1500 #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) #define MAX_QUEUE_SIZE 2 // Private types // Private variables static xTaskHandle taskHandle; static xQueueHandle queue; // Private functions static void flightPlanTask(void *parameters); static void objectUpdatedCb(UAVObjEvent *ev); // External variables (temporary, TODO: this will be loaded from the SD card) extern unsigned char usrlib_img[]; /** * Module initialization */ int32_t FlightPlanStart() { taskHandle = NULL; // Start VM thread xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle); return 0; } /** * Module initialization */ int32_t FlightPlanInitialize() { taskHandle = NULL; FlightPlanStatusInitialize(); FlightPlanControlInitialize(); FlightPlanSettingsInitialize(); // Listen for object updates FlightPlanControlConnectCallback(&objectUpdatedCb); // Create object queue queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); // Listen for FlightPlanControl updates FlightPlanControlConnectQueue(queue); return 0; } MODULE_INITCALL(FlightPlanInitialize, FlightPlanStart); /** * Module task */ static void flightPlanTask(__attribute__((unused)) void *parameters) { UAVObjEvent ev; PmReturn_t retval; FlightPlanStatusData status; FlightPlanControlData control; // Setup status object status.Status = FLIGHTPLANSTATUS_STATUS_STOPPED; status.ErrorFileID = 0; status.ErrorLineNum = 0; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE; status.Debug[0] = 0.0; status.Debug[1] = 0.0; FlightPlanStatusSet(&status); // Main thread loop while (1) { // Wait for FlightPlanControl updates while (xQueueReceive(queue, &ev, portMAX_DELAY) != pdTRUE) { ; } // Get object and check if a start command was sent FlightPlanControlGet(&control); if (control.Command == FLIGHTPLANCONTROL_COMMAND_START) { // Init PyMite retval = pm_init(MEMSPACE_PROG, usrlib_img); if (retval == PM_RET_OK) { // Update status FlightPlanStatusGet(&status); status.Status = FLIGHTPLANSTATUS_STATUS_RUNNING; FlightPlanStatusSet(&status); // Run the test script (TODO: load from SD card) retval = pm_run((uint8_t *)"test"); // Check if an error or exception was thrown if (retval == PM_RET_OK || retval == PM_RET_EX_EXIT) { status.Status = FLIGHTPLANSTATUS_STATUS_STOPPED; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE; } else if (retval == PM_RET_EX) { status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_EXCEPTION; } else if (retval == PM_RET_EX_IO) { status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_IOERROR; } else if (retval == PM_RET_EX_ZDIV) { status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_DIVBYZERO; } else if (retval == PM_RET_EX_ASSRT) { status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_ASSERTERROR; } else if (retval == PM_RET_EX_ATTR) { status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_ATTRIBUTEERROR; } else if (retval == PM_RET_EX_IMPRT) { status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_IMPORTERROR; } else if (retval == PM_RET_EX_INDX) { status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_INDEXERROR; } else if (retval == PM_RET_EX_KEY) { status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_KEYERROR; } else if (retval == PM_RET_EX_MEM) { status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_MEMORYERROR; } else if (retval == PM_RET_EX_NAME) { status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NAMEERROR; } else if (retval == PM_RET_EX_SYNTAX) { status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_SYNTAXERROR; } else if (retval == PM_RET_EX_SYS) { status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_SYSTEMERROR; } else if (retval == PM_RET_EX_TYPE) { status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_TYPEERROR; } else if (retval == PM_RET_EX_VAL) { status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_VALUEERROR; } else if (retval == PM_RET_EX_STOP) { status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_STOPITERATION; } else if (retval == PM_RET_EX_WARN) { status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_WARNING; } else { status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_UNKNOWNERROR; } // Get file ID and line number of error (if one) status.ErrorFileID = gVmGlobal.errFileId; status.ErrorLineNum = gVmGlobal.errLineNum; } else { status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_VMINITERROR; } // Update status object FlightPlanStatusSet(&status); } } } /** * Function called in response to object updates. * Used to force kill the VM thread. */ static void objectUpdatedCb(UAVObjEvent *ev) { FlightPlanControlData controlData; FlightPlanStatusData statusData; // If the object updated was the FlightPlanControl execute requested action if (ev->obj == FlightPlanControlHandle()) { // Get data FlightPlanControlGet(&controlData); // Execute command if (controlData.Command == FLIGHTPLANCONTROL_COMMAND_START) { // Start VM task if not running already if (taskHandle == NULL) { xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle); } } else if (controlData.Command == FLIGHTPLANCONTROL_COMMAND_KILL) { // Force kill VM task if it is already running // (NOTE: the STOP command is preferred as it allows the script to terminate without killing the VM) if (taskHandle != NULL) { // Kill VM PIOS_TASK_MONITOR_UnregisterTask(TASKINFO_RUNNING_FLIGHTPLAN); vTaskDelete(taskHandle); taskHandle = NULL; // Update status object statusData.Status = FLIGHTPLANSTATUS_STATUS_STOPPED; statusData.ErrorFileID = 0; statusData.ErrorLineNum = 0; statusData.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE; statusData.Debug[0] = 0.0; statusData.Debug[1] = 0.0; FlightPlanStatusSet(&statusData); } } } } /** * @} * @} */