From b297ef0ae5ae0fc2383231dd2b0e05614cdfe3bf Mon Sep 17 00:00:00 2001 From: vassilis Date: Sat, 22 Jan 2011 21:32:23 +0000 Subject: [PATCH] Flight/FlightPlanner: First release of openpilot.py library giving access to RTOS and debug fields. Implemented STOP command, this should be the normal way for stopping scripts. The KILL command should be only used as a last resort for misbehaving scripts. The KILL command will delete the VM thread. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2535 ebee16cc-31ac-478f-84a7-5cbb03baadba --- .../PyMite/platform/openpilot/plat.c | 2 + flight/Modules/FlightPlan/flightplan.c | 262 ++++++++++-------- flight/Modules/FlightPlan/flightplans/test.py | 16 +- flight/Modules/FlightPlan/lib/openpilot.py | 209 ++++++++++++++ flight/Modules/FlightPlan/lib/uavobjects.py | 27 -- .../uavobjectdefinition/flightplanstatus.xml | 4 +- 6 files changed, 366 insertions(+), 154 deletions(-) create mode 100644 flight/Modules/FlightPlan/lib/openpilot.py delete mode 100644 flight/Modules/FlightPlan/lib/uavobjects.py diff --git a/flight/Libraries/PyMite/platform/openpilot/plat.c b/flight/Libraries/PyMite/platform/openpilot/plat.c index eee45548e..f68ac7af6 100644 --- a/flight/Libraries/PyMite/platform/openpilot/plat.c +++ b/flight/Libraries/PyMite/platform/openpilot/plat.c @@ -25,6 +25,8 @@ #include "pm.h" #include "openpilot.h" +int pylinenum; + PmReturn_t plat_init(void) { return PM_RET_OK; diff --git a/flight/Modules/FlightPlan/flightplan.c b/flight/Modules/FlightPlan/flightplan.c index 0e0049859..ad9acaa41 100644 --- a/flight/Modules/FlightPlan/flightplan.c +++ b/flight/Modules/FlightPlan/flightplan.c @@ -38,11 +38,13 @@ // 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); @@ -57,9 +59,20 @@ extern unsigned char usrlib_img[]; int32_t FlightPlanInitialize() { taskHandle = NULL; + // Listen for object updates FlightPlanControlConnectCallback(&objectUpdatedCb); + // Create object queue + queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + + // Listen for FlightPlanControl updates + FlightPlanControlConnectQueue(queue); + + // Start VM thread + xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); + TaskMonitorAdd(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle); + return 0; } @@ -68,12 +81,13 @@ int32_t FlightPlanInitialize() */ static void flightPlanTask(void *parameters) { - portTickType lastSysTime; + UAVObjEvent ev; PmReturn_t retval; FlightPlanStatusData status; + FlightPlanControlData control; // Setup status object - status.Status = FLIGHTPLANSTATUS_STATUS_RUNNING; + status.Status = FLIGHTPLANSTATUS_STATUS_STOPPED; status.ErrorFileID = 0; status.ErrorLineNum = 0; status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE; @@ -81,128 +95,136 @@ static void flightPlanTask(void *parameters) status.Debug2 = 0.0; FlightPlanStatusSet(&status); - // Init PyMite - retval = pm_init(MEMSPACE_PROG, usrlib_img); - if (retval == PM_RET_OK) - { - // 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) - { - status.Status = FLIGHTPLANSTATUS_STATUS_FINISHED; - 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_EXIT) - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_EXIT; - } - 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; - } - } - else - { - status.Status = FLIGHTPLANSTATUS_STATUS_ERROR; - status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_VMINITERROR; - } - - // Update status object - FlightPlanStatusSet(&status); - - // Do not return - lastSysTime = xTaskGetTickCount(); + // Main thread loop while (1) { - vTaskDelayUntil(&lastSysTime, 1000 / portTICK_RATE_MS); + // 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 + * Function called in response to object updates. + * Used to force kill the VM thread. */ static void objectUpdatedCb(UAVObjEvent * ev) { @@ -224,10 +246,10 @@ static void objectUpdatedCb(UAVObjEvent * ev) TaskMonitorAdd(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle); } } - else if ( controlData.Command == FLIGHTPLANCONTROL_COMMAND_KILL || controlData.Command == FLIGHTPLANCONTROL_COMMAND_STOP ) + else if ( controlData.Command == FLIGHTPLANCONTROL_COMMAND_KILL ) { // Force kill VM task if it is already running - // (NOTE: when implemented, the STOP command will allow the script to terminate without killing the VM) + // (NOTE: the STOP command is preferred as it allows the script to terminate without killing the VM) if ( taskHandle != NULL ) { // Kill VM diff --git a/flight/Modules/FlightPlan/flightplans/test.py b/flight/Modules/FlightPlan/flightplans/test.py index 374c4764d..d3f566cce 100644 --- a/flight/Modules/FlightPlan/flightplans/test.py +++ b/flight/Modules/FlightPlan/flightplans/test.py @@ -1,9 +1,15 @@ -import uavobjects +import openpilot import sys -n=0 -while n<200000: - uavobjects.FlightPlanStatusUpdate(n) - n=n+1 +n = 0 +timenow = openpilot.time() + +while n < 120: + n = n+1 + openpilot.debug(n, timenow) + timenow = openpilot.delayUntil(timenow, 1000) + if openpilot.hasStopRequest(): + sys.exit() + diff --git a/flight/Modules/FlightPlan/lib/openpilot.py b/flight/Modules/FlightPlan/lib/openpilot.py new file mode 100644 index 000000000..de92e833d --- /dev/null +++ b/flight/Modules/FlightPlan/lib/openpilot.py @@ -0,0 +1,209 @@ +# +# ***************************************************************************** +# * +# * @file openpilot.py +# * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. +# * @brief Python OpenPilot library, gives FlightPlan scripts access to +# * RTOS and other functions +# * +# * @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 +# + +"""__NATIVE__ +#include "openpilot.h" +#include "flightplanstatus.h" +#include "flightplancontrol.h" +""" + +import sys + + +# Get system time in ms +def time(): + return sys.time() + +# Delay (suspend VM thread) for timeToDelayMs ms +def delay(timeToDelayMs): + """__NATIVE__ + pPmObj_t pobj; + PmReturn_t retval; + portTickType timeToDelayTicks; + + // Check number of arguments + if (NATIVE_GET_NUM_ARGS() != 1) + { + PM_RAISE(retval, PM_RET_EX_TYPE); + return retval; + } + + // Get argument + pobj = NATIVE_GET_LOCAL(0); + if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_INT ) + timeToDelayTicks = (portTickType)(((pPmInt_t) pobj)->val) / portTICK_RATE_MS; + else if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_FLT ) + timeToDelayTicks = (portTickType)(((pPmFloat_t) pobj)->val) / portTICK_RATE_MS; + else + { + PM_RAISE(retval, PM_RET_EX_TYPE); + return retval; + } + + // Delay + vTaskDelay(timeToDelayTicks); + + return PM_RET_OK; + """ + pass + +# Same as delay() but will result in more exact periodic execution +# lastWakeTimeMs should be initialized with the system time. +# Example: +# timenow = openpilot.time() +# while 1: +# timenow = openpilot.delayUntil(timenow, 1000) +# +def delayUntil(lastWakeTimeMs, timeToDelayMs): + """__NATIVE__ + pPmObj_t pobj; + pPmObj_t pobjret; + PmReturn_t retval; + portTickType lastWakeTimeTicks; + portTickType timeToDelayTicks; + + // Check number of arguments + if (NATIVE_GET_NUM_ARGS() != 2) + { + PM_RAISE(retval, PM_RET_EX_TYPE); + return retval; + } + + // Get lastWakeTimeMs argument + pobj = NATIVE_GET_LOCAL(0); + if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_INT ) + lastWakeTimeTicks = (portTickType)(((pPmInt_t) pobj)->val) / portTICK_RATE_MS; + else if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_FLT ) + lastWakeTimeTicks = (portTickType)(((pPmFloat_t) pobj)->val) / portTICK_RATE_MS; + else + { + PM_RAISE(retval, PM_RET_EX_TYPE); + return retval; + } + + // Get timeToDelayMs argument + pobj = NATIVE_GET_LOCAL(1); + if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_INT ) + timeToDelayTicks = (portTickType)(((pPmInt_t) pobj)->val) / portTICK_RATE_MS; + else if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_FLT ) + timeToDelayTicks = (portTickType)(((pPmFloat_t) pobj)->val) / portTICK_RATE_MS; + else + { + PM_RAISE(retval, PM_RET_EX_TYPE); + return retval; + } + + // Delay + vTaskDelayUntil(&lastWakeTimeTicks, timeToDelayTicks); + + // Return an int object with the time value */ + retval = int_new((int32_t)(lastWakeTimeTicks*portTICK_RATE_MS), &pobjret); + NATIVE_SET_TOS(pobjret); + return retval; + """ + pass + +# Update FlightPlanStatus debug fields +def debug(val1, val2): + """__NATIVE__ + pPmObj_t pobj; + FlightPlanStatusData status; + PmReturn_t retval; + + // Check number of arguments + if (NATIVE_GET_NUM_ARGS() != 2) + { + PM_RAISE(retval, PM_RET_EX_TYPE); + return retval; + } + + // Get status object + FlightPlanStatusGet(&status); + + // Update debug1 + pobj = NATIVE_GET_LOCAL(0); + if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_INT ) + status.Debug1 = (float)(((pPmInt_t) pobj)->val); + else if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_FLT ) + status.Debug1 = (float)(((pPmFloat_t) pobj)->val); + else + { + PM_RAISE(retval, PM_RET_EX_TYPE); + return retval; + } + + // Update debug2 + pobj = NATIVE_GET_LOCAL(1); + if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_INT ) + status.Debug2 = (float)(((pPmInt_t) pobj)->val); + else if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_FLT ) + status.Debug2 = (float)(((pPmFloat_t) pobj)->val); + else + { + PM_RAISE(retval, PM_RET_EX_TYPE); + return retval; + } + + // Update status object + FlightPlanStatusSet(&status); + + return PM_RET_OK; + """ + pass + +# Returns 1 if a stop request is pending. The script should periodically check +# for stop requests and exit using sys.exit() if one is detected. +def hasStopRequest(): + """__NATIVE__ + pPmObj_t pobjret; + PmReturn_t retval; + FlightPlanControlData control; + uint32_t stopRequest; + + // Get control object + FlightPlanControlGet(&control); + + // Check if a stop request is pending + if (control.Command == FLIGHTPLANCONTROL_COMMAND_STOP) + stopRequest = 1; + else + stopRequest = 0; + + // Return + retval = int_new((int32_t)(stopRequest), &pobjret); + NATIVE_SET_TOS(pobjret); + return retval; + """ + pass + +# TODO: Wait for object updates in the event queue +def waitForObjectUpdates(timeoutMs): + pass + + + + diff --git a/flight/Modules/FlightPlan/lib/uavobjects.py b/flight/Modules/FlightPlan/lib/uavobjects.py deleted file mode 100644 index 4638a246b..000000000 --- a/flight/Modules/FlightPlan/lib/uavobjects.py +++ /dev/null @@ -1,27 +0,0 @@ -"""__NATIVE__ -#include "openpilot.h" -#include "flightplanstatus.h" -""" - -def FlightPlanStatusUpdate(val): - """__NATIVE__ - pPmObj_t pobj; - FlightPlanStatusData status; - - FlightPlanStatusGet(&status); - - pobj = NATIVE_GET_LOCAL(0); - if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_INT ) - status.Debug1 = ((pPmInt_t) pobj)->val; - else if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_FLT ) - status.Debug1 = ((pPmFloat_t) pobj)->val; - - FlightPlanStatusSet(&status); - - return PM_RET_OK; - """ - pass - - - - diff --git a/shared/uavobjectdefinition/flightplanstatus.xml b/shared/uavobjectdefinition/flightplanstatus.xml index 959b9fe3e..40a1fc3c4 100644 --- a/shared/uavobjectdefinition/flightplanstatus.xml +++ b/shared/uavobjectdefinition/flightplanstatus.xml @@ -1,8 +1,8 @@ Status of the flight plan script - - + +