flight/modules/FlightPlan/flightplan.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup FlightPlan Flight Plan Module
   6  * @brief Executes flight plan scripts in Python
   7  * @{
   8  *
   9  * @file       flightplan.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  11  * @brief      Executes flight plan scripts in Python
  12  *
  13  * @see        The GNU Public License (GPL) Version 3
  14  *
  15  *****************************************************************************/
  16 /*
  17  * This program is free software; you can redistribute it and/or modify
  18  * it under the terms of the GNU General Public License as published by
  19  * the Free Software Foundation; either version 3 of the License, or
  20  * (at your option) any later version.
  21  *
  22  * This program is distributed in the hope that it will be useful, but
  23  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  24  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  25  * for more details.
  26  *
  27  * You should have received a copy of the GNU General Public License along
  28  * with this program; if not, write to the Free Software Foundation, Inc.,
  29  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  30  */
  31
  32 #include <openpilot.h>
  33
  34 #include "flightplanstatus.h"
  35 #include "flightplancontrol.h"
  36 #include "flightplansettings.h"
  37 #include "taskinfo.h"
  38
  39 #include "pm.h"
  40
  41 // Private constants
  42 #define STACK_SIZE_BYTES 1500
  43 #define TASK_PRIORITY    (tskIDLE_PRIORITY + 1)
  44 #define MAX_QUEUE_SIZE   2
  45
  46 // Private types
  47
  48 // Private variables
  49 static xTaskHandle taskHandle;
  50 static xQueueHandle queue;
  51
  52 // Private functions
  53 static void flightPlanTask(void *parameters);
  54 static void objectUpdatedCb(UAVObjEvent *ev);
  55
  56 // External variables (temporary, TODO: this will be loaded from the SD card)
  57 extern unsigned char usrlib_img[];
  58
  59 /**
  60  * Module initialization
  61  */
  62 int32_t FlightPlanStart()
  63 {
  64     taskHandle = NULL;
  65
  66     // Start VM thread
  67     xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
  68     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle);
  69
  70     return 0;
  71 }
  72
  73 /**
  74  * Module initialization
  75  */
  76 int32_t FlightPlanInitialize()
  77 {
  78     taskHandle = NULL;
  79
  80     FlightPlanStatusInitialize();
  81     FlightPlanControlInitialize();
  82
  83     // Listen for object updates
  84     FlightPlanControlConnectCallback(&objectUpdatedCb);
  85
  86     // Create object queue
  87     queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
  88
  89     // Listen for FlightPlanControl updates
  90     FlightPlanControlConnectQueue(queue);
  91
  92     return 0;
  93 }
  94 MODULE_INITCALL(FlightPlanInitialize, FlightPlanStart);
  95 /**
  96  * Module task
  97  */
  98 static void flightPlanTask(__attribute__((unused)) void *parameters)
  99 {
 100     UAVObjEvent ev;
 101     PmReturn_t retval;
 102     FlightPlanStatusData status;
 103     FlightPlanControlData control;
 104
 105     // Setup status object
 106     status.Status       = FLIGHTPLANSTATUS_STATUS_STOPPED;
 107     status.ErrorFileID  = 0;
 108     status.ErrorLineNum = 0;
 109     status.ErrorType    = FLIGHTPLANSTATUS_ERRORTYPE_NONE;
 110     status.Debug[0]     = 0.0;
 111     status.Debug[1]     = 0.0;
 112     FlightPlanStatusSet(&status);
 113
 114     // Main thread loop
 115     while (1) {
 116         // Wait for FlightPlanControl updates
 117         while (xQueueReceive(queue, &ev, portMAX_DELAY) != pdTRUE) {
 118             ;
 119         }
 120
 121         // Get object and check if a start command was sent
 122         FlightPlanControlGet(&control);
 123         if (control.Command == FLIGHTPLANCONTROL_COMMAND_START) {
 124             // Init PyMite
 125             retval = pm_init(MEMSPACE_PROG, usrlib_img);
 126             if (retval == PM_RET_OK) {
 127                 // Update status
 128                 FlightPlanStatusGet(&status);
 129                 status.Status = FLIGHTPLANSTATUS_STATUS_RUNNING;
 130                 FlightPlanStatusSet(&status);
 131                 // Run the test script (TODO: load from SD card)
 132                 retval = pm_run((uint8_t *)"test");
 133                 // Check if an error or exception was thrown
 134                 if (retval == PM_RET_OK || retval == PM_RET_EX_EXIT) {
 135                     status.Status    = FLIGHTPLANSTATUS_STATUS_STOPPED;
 136                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE;
 137                 } else if (retval == PM_RET_EX) {
 138                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 139                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_EXCEPTION;
 140                 } else if (retval == PM_RET_EX_IO) {
 141                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 142                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_IOERROR;
 143                 } else if (retval == PM_RET_EX_ZDIV) {
 144                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 145                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_DIVBYZERO;
 146                 } else if (retval == PM_RET_EX_ASSRT) {
 147                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 148                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_ASSERTERROR;
 149                 } else if (retval == PM_RET_EX_ATTR) {
 150                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 151                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_ATTRIBUTEERROR;
 152                 } else if (retval == PM_RET_EX_IMPRT) {
 153                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 154                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_IMPORTERROR;
 155                 } else if (retval == PM_RET_EX_INDX) {
 156                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 157                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_INDEXERROR;
 158                 } else if (retval == PM_RET_EX_KEY) {
 159                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 160                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_KEYERROR;
 161                 } else if (retval == PM_RET_EX_MEM) {
 162                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 163                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_MEMORYERROR;
 164                 } else if (retval == PM_RET_EX_NAME) {
 165                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 166                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NAMEERROR;
 167                 } else if (retval == PM_RET_EX_SYNTAX) {
 168                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 169                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_SYNTAXERROR;
 170                 } else if (retval == PM_RET_EX_SYS) {
 171                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 172                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_SYSTEMERROR;
 173                 } else if (retval == PM_RET_EX_TYPE) {
 174                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 175                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_TYPEERROR;
 176                 } else if (retval == PM_RET_EX_VAL) {
 177                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 178                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_VALUEERROR;
 179                 } else if (retval == PM_RET_EX_STOP) {
 180                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 181                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_STOPITERATION;
 182                 } else if (retval == PM_RET_EX_WARN) {
 183                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 184                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_WARNING;
 185                 } else {
 186                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 187                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_UNKNOWNERROR;
 188                 }
 189                 // Get file ID and line number of error (if one)
 190                 status.ErrorFileID  = gVmGlobal.errFileId;
 191                 status.ErrorLineNum = gVmGlobal.errLineNum;
 192             } else {
 193                 status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 194                 status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_VMINITERROR;
 195             }
 196
 197             // Update status object
 198             FlightPlanStatusSet(&status);
 199         }
 200     }
 201 }
 202
 203 /**
 204  * Function called in response to object updates.
 205  * Used to force kill the VM thread.
 206  */
 207 static void objectUpdatedCb(UAVObjEvent *ev)
 208 {
 209     FlightPlanControlData controlData;
 210     FlightPlanStatusData statusData;
 211
 212     // If the object updated was the FlightPlanControl execute requested action
 213     if (ev->obj == FlightPlanControlHandle()) {
 214         // Get data
 215         FlightPlanControlGet(&controlData);
 216         // Execute command
 217         if (controlData.Command == FLIGHTPLANCONTROL_COMMAND_START) {
 218             // Start VM task if not running already
 219             if (taskHandle == NULL) {
 220                 xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
 221                 PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle);
 222             }
 223         } else if (controlData.Command == FLIGHTPLANCONTROL_COMMAND_KILL) {
 224             // Force kill VM task if it is already running
 225             // (NOTE: the STOP command is preferred as it allows the script to terminate without killing the VM)
 226             if (taskHandle != NULL) {
 227                 // Kill VM
 228                 PIOS_TASK_MONITOR_UnregisterTask(TASKINFO_RUNNING_FLIGHTPLAN);
 229                 vTaskDelete(taskHandle);
 230                 taskHandle = NULL;
 231                 // Update status object
 232                 statusData.Status       = FLIGHTPLANSTATUS_STATUS_STOPPED;
 233                 statusData.ErrorFileID  = 0;
 234                 statusData.ErrorLineNum = 0;
 235                 statusData.ErrorType    = FLIGHTPLANSTATUS_ERRORTYPE_NONE;
 236                 statusData.Debug[0]     = 0.0;
 237                 statusData.Debug[1]     = 0.0;
 238                 FlightPlanStatusSet(&statusData);
 239             }
 240         }
 241     }
 242 }
 243
 244 /**
 245  * @}
 246  * @}
 247  */