1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-05 13:24:11 +01:00
LibrePilot/flight/modules/FlightPlan/lib/openpilot.py
2013-04-25 13:13:36 +03:00

203 lines
5.2 KiB
Python

#
# *****************************************************************************
# *
# * @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"
"""
# 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.Debug[0] = (float)(((pPmInt_t) pobj)->val);
else if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_FLT )
status.Debug[0] = (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.Debug[1] = (float)(((pPmInt_t) pobj)->val);
else if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_FLT )
status.Debug[1] = (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