mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-29 07:24:13 +01:00
250 lines
9.7 KiB
C
250 lines
9.7 KiB
C
/**
|
|
******************************************************************************
|
|
* @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 "flightplan.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);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @}
|
|
* @}
|
|
*/
|