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     FlightPlanSettingsInitialize();
  83
  84     // Listen for object updates
  85     FlightPlanControlConnectCallback(&objectUpdatedCb);
  86
  87     // Create object queue
  88     queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
  89
  90     // Listen for FlightPlanControl updates
  91     FlightPlanControlConnectQueue(queue);
  92
  93     return 0;
  94 }
  95 MODULE_INITCALL(FlightPlanInitialize, FlightPlanStart);
  96 /**
  97  * Module task
  98  */
  99 static void flightPlanTask(__attribute__((unused)) void *parameters)
 100 {
 101     UAVObjEvent ev;
 102     PmReturn_t retval;
 103     FlightPlanStatusData status;
 104     FlightPlanControlData control;
 105
 106     // Setup status object
 107     status.Status       = FLIGHTPLANSTATUS_STATUS_STOPPED;
 108     status.ErrorFileID  = 0;
 109     status.ErrorLineNum = 0;
 110     status.ErrorType    = FLIGHTPLANSTATUS_ERRORTYPE_NONE;
 111     status.Debug[0]     = 0.0;
 112     status.Debug[1]     = 0.0;
 113     FlightPlanStatusSet(&status);
 114
 115     // Main thread loop
 116     while (1) {
 117         // Wait for FlightPlanControl updates
 118         while (xQueueReceive(queue, &ev, portMAX_DELAY) != pdTRUE) {
 119             ;
 120         }
 121
 122         // Get object and check if a start command was sent
 123         FlightPlanControlGet(&control);
 124         if (control.Command == FLIGHTPLANCONTROL_COMMAND_START) {
 125             // Init PyMite
 126             retval = pm_init(MEMSPACE_PROG, usrlib_img);
 127             if (retval == PM_RET_OK) {
 128                 // Update status
 129                 FlightPlanStatusGet(&status);
 130                 status.Status = FLIGHTPLANSTATUS_STATUS_RUNNING;
 131                 FlightPlanStatusSet(&status);
 132                 // Run the test script (TODO: load from SD card)
 133                 retval = pm_run((uint8_t *)"test");
 134                 // Check if an error or exception was thrown
 135                 if (retval == PM_RET_OK || retval == PM_RET_EX_EXIT) {
 136                     status.Status    = FLIGHTPLANSTATUS_STATUS_STOPPED;
 137                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE;
 138                 } else if (retval == PM_RET_EX) {
 139                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 140                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_EXCEPTION;
 141                 } else if (retval == PM_RET_EX_IO) {
 142                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 143                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_IOERROR;
 144                 } else if (retval == PM_RET_EX_ZDIV) {
 145                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 146                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_DIVBYZERO;
 147                 } else if (retval == PM_RET_EX_ASSRT) {
 148                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 149                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_ASSERTERROR;
 150                 } else if (retval == PM_RET_EX_ATTR) {
 151                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 152                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_ATTRIBUTEERROR;
 153                 } else if (retval == PM_RET_EX_IMPRT) {
 154                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 155                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_IMPORTERROR;
 156                 } else if (retval == PM_RET_EX_INDX) {
 157                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 158                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_INDEXERROR;
 159                 } else if (retval == PM_RET_EX_KEY) {
 160                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 161                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_KEYERROR;
 162                 } else if (retval == PM_RET_EX_MEM) {
 163                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 164                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_MEMORYERROR;
 165                 } else if (retval == PM_RET_EX_NAME) {
 166                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 167                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NAMEERROR;
 168                 } else if (retval == PM_RET_EX_SYNTAX) {
 169                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 170                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_SYNTAXERROR;
 171                 } else if (retval == PM_RET_EX_SYS) {
 172                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 173                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_SYSTEMERROR;
 174                 } else if (retval == PM_RET_EX_TYPE) {
 175                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 176                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_TYPEERROR;
 177                 } else if (retval == PM_RET_EX_VAL) {
 178                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 179                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_VALUEERROR;
 180                 } else if (retval == PM_RET_EX_STOP) {
 181                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 182                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_STOPITERATION;
 183                 } else if (retval == PM_RET_EX_WARN) {
 184                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 185                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_WARNING;
 186                 } else {
 187                     status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 188                     status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_UNKNOWNERROR;
 189                 }
 190                 // Get file ID and line number of error (if one)
 191                 status.ErrorFileID  = gVmGlobal.errFileId;
 192                 status.ErrorLineNum = gVmGlobal.errLineNum;
 193             } else {
 194                 status.Status    = FLIGHTPLANSTATUS_STATUS_ERROR;
 195                 status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_VMINITERROR;
 196             }
 197
 198             // Update status object
 199             FlightPlanStatusSet(&status);
 200         }
 201     }
 202 }
 203
 204 /**
 205  * Function called in response to object updates.
 206  * Used to force kill the VM thread.
 207  */
 208 static void objectUpdatedCb(UAVObjEvent *ev)
 209 {
 210     FlightPlanControlData controlData;
 211     FlightPlanStatusData statusData;
 212
 213     // If the object updated was the FlightPlanControl execute requested action
 214     if (ev->obj == FlightPlanControlHandle()) {
 215         // Get data
 216         FlightPlanControlGet(&controlData);
 217         // Execute command
 218         if (controlData.Command == FLIGHTPLANCONTROL_COMMAND_START) {
 219             // Start VM task if not running already
 220             if (taskHandle == NULL) {
 221                 xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
 222                 PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle);
 223             }
 224         } else if (controlData.Command == FLIGHTPLANCONTROL_COMMAND_KILL) {
 225             // Force kill VM task if it is already running
 226             // (NOTE: the STOP command is preferred as it allows the script to terminate without killing the VM)
 227             if (taskHandle != NULL) {
 228                 // Kill VM
 229                 PIOS_TASK_MONITOR_UnregisterTask(TASKINFO_RUNNING_FLIGHTPLAN);
 230                 vTaskDelete(taskHandle);
 231                 taskHandle = NULL;
 232                 // Update status object
 233                 statusData.Status       = FLIGHTPLANSTATUS_STATUS_STOPPED;
 234                 statusData.ErrorFileID  = 0;
 235                 statusData.ErrorLineNum = 0;
 236                 statusData.ErrorType    = FLIGHTPLANSTATUS_ERRORTYPE_NONE;
 237                 statusData.Debug[0]     = 0.0;
 238                 statusData.Debug[1]     = 0.0;
 239                 FlightPlanStatusSet(&statusData);
 240             }
 241         }
 242     }
 243 }
 244
 245 /**
 246  * @}
 247  * @}
 248  */