Flight/FlightPlan: Enable Python VM and test flightplan execution. From now on a Python executable needs to be in the system path as it is used in the Makefile.

A test script is statically linked (will eventually be uploaded by the GCS and stored in the VM), it can be found under: Modules/FlightPlan/flightplans/test.py To start the script send the FlightPlanControl object with the Start command, to stop send the Stop or Kill commands. Next release will be the OpenPilot python libraries and access to UAVObjects from the script. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2480 ebee16cc-31ac-478f-84a7-5cbb03baadba
2024-12-01 09:24:10 +01:00 · 2011-01-19 03:34:01 +00:00 · 2011-01-19 03:34:01 +00:00 · d7fd8a3bcd
commit d7fd8a3bcd
parent 780f441ae9
8 changed files with 212 additions and 45 deletions
--- a/flight/Modules/FlightPlan/flightplan.c
+++ b/flight/Modules/FlightPlan/flightplan.c
@ -32,10 +32,12 @@
 #include "openpilot.h"
 #include "pm.h"
 #include "flightplanstatus.h"
+#include "flightplancontrol.h"
+#include "flightplansettings.h"

 // Private constants
 #define STACK_SIZE_BYTES 1500
-#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
+#define TASK_PRIORITY (tskIDLE_PRIORITY+1)

 // Private types

@ -44,6 +46,7 @@ static xTaskHandle taskHandle;

 // 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[];
@ -53,9 +56,9 @@ extern unsigned char usrlib_img[];
 */
 int32_t FlightPlanInitialize()
 {
-	// Start main task
-	xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
-	TaskMonitorAdd(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle);
+	taskHandle = NULL;
+	// Listen for object updates
+	FlightPlanControlConnectCallback(&objectUpdatedCb);

 	return 0;
 }
@ -70,30 +73,125 @@ static void flightPlanTask(void *parameters)
 	FlightPlanStatusData status;

 	// Setup status object
-	status.Status = FLIGHTPLANSTATUS_STATUS_NONE;
+	status.Status = FLIGHTPLANSTATUS_STATUS_RUNNING;
 	status.ErrorFileID = 0;
 	status.ErrorLineNum = 0;
 	status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE;
-	status.Debug = 0.0;
+	status.Debug1 = 0.0;
+	status.Debug2 = 0.0;
+	FlightPlanStatusSet(&status);

    // Init PyMite
-	status.Status = FLIGHTPLANSTATUS_STATUS_IDLE;
    retval = pm_init(MEMSPACE_PROG, usrlib_img);
-    if (retval != PM_RET_OK)
+    if (retval == PM_RET_OK)
    {
-    	status.Status = FLIGHTPLANSTATUS_STATUS_VMINITERROR;
+		// 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;
    }

-    // Run the sample program
-    status.Status = FLIGHTPLANSTATUS_STATUS_RUNNING;
+    // Update status object
    FlightPlanStatusSet(&status);
-    retval = pm_run((uint8_t *)"test");
-    if (retval != PM_RET_OK)
-    {
-    	status.Status = FLIGHTPLANSTATUS_STATUS_SCRIPTSTARTERROR;
-    	status.Debug = retval;
-    	FlightPlanStatusSet(&status);
-    }

    // Do not return
    lastSysTime = xTaskGetTickCount();
@ -101,8 +199,52 @@ static void flightPlanTask(void *parameters)
 	{
 		vTaskDelayUntil(&lastSysTime, 1000 / portTICK_RATE_MS);
 	}
+}

+/**
+ * Function called in response to object updates
+ */
+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);
+				TaskMonitorAdd(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle);
+			}
+		}
+		else if ( controlData.Command == FLIGHTPLANCONTROL_COMMAND_KILL || controlData.Command == FLIGHTPLANCONTROL_COMMAND_STOP )
+		{
+			// 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)
+			if ( taskHandle != NULL )
+			{
+				// Kill VM
+				TaskMonitorRemove(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.Debug1 = 0.0;
+				statusData.Debug2 = 0.0;
+				FlightPlanStatusSet(&statusData);
+			}
+		}
+	}
 }

 /**
--- a/flight/Modules/FlightPlan/flightplans/test.py
+++ b/flight/Modules/FlightPlan/flightplans/test.py
@ -1,6 +1,9 @@
 import uavobjects
 import sys

-uavobjects.FlightPlanStatusUpdate(12) 
+n=0
+while n<200000:
+	uavobjects.FlightPlanStatusUpdate(n)
+	n=n+1 


--- a/flight/Modules/FlightPlan/lib/uavobjects.py
+++ b/flight/Modules/FlightPlan/lib/uavobjects.py
@ -7,12 +7,14 @@ 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.Debug = ((pPmInt_t) pobj)->val;
+		status.Debug1 = ((pPmInt_t) pobj)->val;
 	else if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_FLT )
-		status.Debug = ((pPmFloat_t) pobj)->val;
+		status.Debug1 = ((pPmFloat_t) pobj)->val;
 	 
 	FlightPlanStatusSet(&status);
 
--- a/flight/OpenPilot/Makefile
+++ b/flight/OpenPilot/Makefile
@ -57,7 +57,7 @@ FLASH_TOOL = OPENOCD
 USE_THUMB_MODE = YES

 # List of modules to include
-MODULES =  Actuator Telemetry GPS ManualControl Altitude AHRSComms Stabilization Guidance FirmwareIAP
+MODULES =  Actuator Telemetry GPS ManualControl Altitude AHRSComms Stabilization Guidance FirmwareIAP FlightPlan

 #MODULES = Telemetry Example
 #MODULES = Telemetry MK/MKSerial
@ -298,12 +298,12 @@ SRC += $(AHRSBOOTLOADER)/ahrs_spi_program_master.c
 SRC += $(AHRSBOOTLOADER)/ahrs_spi_program.c

 ## PyMite files
-#SRC += $(wildcard ${PYMITEVM}/*.c)
-#SRC += $(wildcard ${PYMITEPLAT}/*.c)
-#SRC += $(OUTDIR)/pmlib_img.c
-#SRC += $(OUTDIR)/pmlib_nat.c
-#SRC += $(OUTDIR)/pmlibusr_img.c
-#SRC += $(OUTDIR)/pmlibusr_nat.c
+SRC += $(OUTDIR)/pmlib_img.c
+SRC += $(OUTDIR)/pmlib_nat.c
+SRC += $(OUTDIR)/pmlibusr_img.c
+SRC += $(OUTDIR)/pmlibusr_nat.c
+SRC += $(wildcard ${PYMITEVM}/*.c)
+SRC += $(wildcard ${PYMITEPLAT}/*.c)

 ## Mass Storage Device
 #SRC += $(MSDDIR)/msd.c
@ -614,7 +614,7 @@ else
 endif

 # Generate intermediate code
-gencode: ${OUTDIR}/InitMods.c #$(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h
+gencode: ${OUTDIR}/InitMods.c ${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h 

 # Generate code for module initialization
 ${OUTDIR}/InitMods.c: Makefile
@ -626,11 +626,11 @@ ${OUTDIR}/InitMods.c: Makefile
 	@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c

 # Generate code for PyMite
-#$(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h: $(wildcard $(PYMITELIB)/*.py) $(wildcard $(PYMITEPLAT)/*.py) $(wildcard $(FLIGHTPLANLIB)/*.py) $(wildcard $(FLIGHTPLANS)/*.py) 
-#	@echo ${MSG_PYMITEINIT}
-#	@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py)
-#	@$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h
-#	@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -u -o $(OUTDIR)/pmlibusr_img.c --native-file=$(OUTDIR)/pmlibusr_nat.c $(FLIGHTPLANS)/test.py
+${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h: $(wildcard ${PYMITELIB}/*.py) $(wildcard ${PYMITEPLAT}/*.py) $(wildcard ${FLIGHTPLANLIB}/*.py) $(wildcard ${FLIGHTPLANS}/*.py) 
+	@echo ${MSG_PYMITEINIT}
+	@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py)
+	@$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h
+	@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -u -o $(OUTDIR)/pmlibusr_img.c --native-file=$(OUTDIR)/pmlibusr_nat.c $(FLIGHTPLANS)/test.py

 # Eye candy.
 begin:
--- a/flight/OpenPilot/System/inc/taskmonitor.h
+++ b/flight/OpenPilot/System/inc/taskmonitor.h
@ -32,6 +32,7 @@

 int32_t TaskMonitorInitialize(void);
 int32_t TaskMonitorAdd(TaskInfoRunningElem task, xTaskHandle handle);
+int32_t TaskMonitorRemove(TaskInfoRunningElem task);
 void TaskMonitorUpdateAll(void);

 #endif // TASKMONITOR_H
--- a/flight/OpenPilot/System/taskmonitor.c
+++ b/flight/OpenPilot/System/taskmonitor.c
@ -66,6 +66,24 @@ int32_t TaskMonitorAdd(TaskInfoRunningElem task, xTaskHandle handle)
 	}
 }

+/**
+ * Remove a task handle from the library
+ */
+int32_t TaskMonitorRemove(TaskInfoRunningElem task)
+{
+	if (task < TASKINFO_RUNNING_NUMELEM)
+	{
+	    xSemaphoreTakeRecursive(lock, portMAX_DELAY);
+		handles[task] = 0;
+		xSemaphoreGiveRecursive(lock);
+		return 0;
+	}
+	else
+	{
+		return -1;
+	}
+}
+
 /**
 * Update the status of all tasks
 */
--- a/ground/src/shared/uavobjectdefinition/flightplancontrol.xml
+++ b/ground/src/shared/uavobjectdefinition/flightplancontrol.xml
@ -1,9 +1,9 @@
 <xml>
    <object name="FlightPlanControl" singleinstance="true" settings="false">
        <description>Control the flight plan script</description>
-        <field name="Test" units="%" type="float" elements="1"/>
+        <field name="Command" units="" type="enum" options="Start,Stop,Kill" defaultvalue="Start" elements="1"/>
        <access gcs="readwrite" flight="readwrite"/>
-        <telemetrygcs acked="true" updatemode="onchange" period="0"/>
+        <telemetrygcs acked="true" updatemode="manual" period="0"/>
        <telemetryflight acked="true" updatemode="manual" period="0"/>
        <logging updatemode="never" period="0"/>
    </object>
--- a/ground/src/shared/uavobjectdefinition/flightplanstatus.xml
+++ b/ground/src/shared/uavobjectdefinition/flightplanstatus.xml
@ -1,14 +1,15 @@
 <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="None,Running,Idle,VMInitError,ScriptStartError,RunTimeError" defaultvalue="None"/>
-        <field name="ErrorType" units="" type="enum" elements="1" options="None" defaultvalue="None"/>
+		<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="ErrorFileID" units="" type="uint32" elements="1"/>
 		<field name="ErrorLineNum" units="" type="uint32" elements="1"/>
-		<field name="Debug" units="" type="float" elements="1"/>
-        <access gcs="readwrite" flight="readwrite"/>
-        <telemetrygcs acked="false" updatemode="manual" period="0"/>
-        <telemetryflight acked="false" updatemode="periodic" period="2000"/>
-        <logging updatemode="never" period="0"/>
+		<field name="Debug1" units="" type="float" elements="1" defaultvalue="0.0"/>
+		<field name="Debug2" units="" type="float" elements="1" defaultvalue="0.0"/>
+    <access gcs="readwrite" flight="readwrite"/>
+    <telemetrygcs acked="false" updatemode="manual" period="0"/>
+    <telemetryflight acked="false" updatemode="periodic" period="2000"/>
+    <logging updatemode="never" period="0"/>
    </object>
 </xml>