rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00
Code Issues Releases Activity

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.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2535 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
vassilis 2011-01-22 21:32:23 +00:00 committed by vassilis
parent 81c5ee2850
commit b297ef0ae5
6 changed files with 366 additions and 154 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/Libraries/PyMite/platform/openpilot/plat.c
View File

@ -25,6 +25,8 @@
#include "pm.h"
#include "openpilot.h"
int pylinenum;
PmReturn_t plat_init(void)
{
return PM_RET_OK;

262
flight/Modules/FlightPlan/flightplan.c
View File

@ -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

16
flight/Modules/FlightPlan/flightplans/test.py
View File

@ -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()

209
flight/Modules/FlightPlan/lib/openpilot.py Normal file
View File

@ -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

27
flight/Modules/FlightPlan/lib/uavobjects.py
View File

@ -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

4
shared/uavobjectdefinition/flightplanstatus.xml
View File

@ -1,8 +1,8 @@
<xml>
<object name="FlightPlanStatus" singleinstance="true" settings="false">
<description>Status of the flight plan script</description>
<field name="Status" units="" type="enum" elements="1" options="Stopped,Running,Finished,Error" defaultvalue="Stopped"/>
<field name="ErrorType" units="" type="enum" elements="1" options="None,VMInitError,Exception,Exit,IOError,DivByZero,AssertError,AttributeError,ImportError,IndexError,KeyError,MemoryError,NameError,SyntaxError,SystemError,TypeError,ValueError,StopIteration,Warning,UnknownError" defaultvalue="None"/>
<field name="Status" units="" type="enum" elements="1" options="Stopped,Running,Error" defaultvalue="Stopped"/>
<field name="ErrorType" units="" type="enum" elements="1" options="None,VMInitError,Exception,IOError,DivByZero,AssertError,AttributeError,ImportError,IndexError,KeyError,MemoryError,NameError,SyntaxError,SystemError,TypeError,ValueError,StopIteration,Warning,UnknownError" defaultvalue="None"/>
<field name="ErrorFileID" units="" type="uint32" elements="1"/>
<field name="ErrorLineNum" units="" type="uint32" elements="1"/>
<field name="Debug1" units="" type="float" elements="1" defaultvalue="0.0"/>