/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup FlightPlan Flight Plan Module
* @brief Executes flight plan scripts in Python
* @{
*
* @file flightplan.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Executes flight plan scripts in Python
*
* @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
*/
#include <openpilot.h>
#include "flightplanstatus.h"
#include "flightplancontrol.h"
#include "flightplansettings.h"
#include "taskinfo.h"
#include "pm.h"
// 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);
static void objectUpdatedCb(UAVObjEvent *ev);
// External variables (temporary, TODO: this will be loaded from the SD card)
extern unsigned char usrlib_img[];
/**
* Module initialization
*/
int32_t FlightPlanStart()
{
taskHandle = NULL;
// Start VM thread
xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle);
return 0;
}
/**
* Module initialization
*/
int32_t FlightPlanInitialize()
{
taskHandle = NULL;
FlightPlanStatusInitialize();
FlightPlanControlInitialize();
FlightPlanSettingsInitialize();
// Listen for object updates
FlightPlanControlConnectCallback(&objectUpdatedCb);
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// Listen for FlightPlanControl updates
FlightPlanControlConnectQueue(queue);
return 0;
}
MODULE_INITCALL(FlightPlanInitialize, FlightPlanStart);
/**
* Module task
*/
static void flightPlanTask(__attribute__((unused)) void *parameters)
{
UAVObjEvent ev;
PmReturn_t retval;
FlightPlanStatusData status;
FlightPlanControlData control;
// Setup status object
status.Status = FLIGHTPLANSTATUS_STATUS_STOPPED;
status.ErrorFileID = 0;
status.ErrorLineNum = 0;
status.ErrorType = FLIGHTPLANSTATUS_ERRORTYPE_NONE;
status.Debug[0] = 0.0;
status.Debug[1] = 0.0;
FlightPlanStatusSet(&status);
// Main thread loop
while (1) {
// 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.
* Used to force kill the VM thread.
*/
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);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle);
}
} else if (controlData.Command == FLIGHTPLANCONTROL_COMMAND_KILL) {
// Force kill VM task if it is already running
// (NOTE: the STOP command is preferred as it allows the script to terminate without killing the VM)
if (taskHandle != NULL) {
// Kill VM
PIOS_TASK_MONITOR_UnregisterTask(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.Debug[0] = 0.0;
statusData.Debug[1] = 0.0;
FlightPlanStatusSet(&statusData);
}
}
}
}
/**
* @}
* @}
*/