flight/modules/FlightPlan/lib/openpilot.py

   1 #
   2 # *****************************************************************************
   3 # *
   4 # * @file       openpilot.py
   5 # * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
   6 # * @brief      Python OpenPilot library, gives FlightPlan scripts access to
   7 # *             RTOS and other functions
   8 # *
   9 # * @see        The GNU Public License (GPL) Version 3
  10 # *
  11 # *****************************************************************************
  12 # *
  13 # * This program is free software; you can redistribute it and/or modify
  14 # * it under the terms of the GNU General Public License as published by
  15 # * the Free Software Foundation; either version 3 of the License, or
  16 # * (at your option) any later version.
  17 # *
  18 # * This program is distributed in the hope that it will be useful, but
  19 # * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  20 # * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  21 # * for more details.
  22 # *
  23 # * You should have received a copy of the GNU General Public License along
  24 # * with this program; if not, write to the Free Software Foundation, Inc.,
  25 # * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  26 #
  27
  28 """__NATIVE__
  29 #include "openpilot.h"
  30 #include "flightplanstatus.h"
  31 #include "flightplancontrol.h"
  32 """
  33
  34 # Delay (suspend VM thread) for timeToDelayMs ms
  35 def delay(timeToDelayMs):
  36         """__NATIVE__
  37         pPmObj_t pobj;
  38         PmReturn_t retval;
  39         portTickType timeToDelayTicks;
  40
  41         // Check number of arguments
  42   if (NATIVE_GET_NUM_ARGS() != 1)
  43   {
  44                 PM_RAISE(retval, PM_RET_EX_TYPE);
  45                 return retval;
  46         }
  47
  48         // Get argument
  49         pobj = NATIVE_GET_LOCAL(0);
  50         if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_INT )
  51                 timeToDelayTicks = (portTickType)(((pPmInt_t) pobj)->val) / portTICK_RATE_MS;
  52         else if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_FLT )
  53                 timeToDelayTicks = (portTickType)(((pPmFloat_t) pobj)->val) / portTICK_RATE_MS;
  54         else
  55         {
  56                 PM_RAISE(retval, PM_RET_EX_TYPE);
  57                 return retval;
  58         }
  59
  60         // Delay
  61         vTaskDelay(timeToDelayTicks);
  62
  63         return PM_RET_OK;
  64         """
  65         pass
  66
  67 # Same as delay() but will result in more exact periodic execution
  68 # lastWakeTimeMs should be initialized with the system time.
  69 # Example:
  70 # timenow = openpilot.time()
  71 # while 1:
  72 #         timenow = openpilot.delayUntil(timenow, 1000)
  73 #
  74 def delayUntil(lastWakeTimeMs, timeToDelayMs):
  75         """__NATIVE__
  76         pPmObj_t pobj;
  77         pPmObj_t pobjret;
  78         PmReturn_t retval;
  79         portTickType lastWakeTimeTicks;
  80         portTickType timeToDelayTicks;
  81
  82         // Check number of arguments
  83   if (NATIVE_GET_NUM_ARGS() != 2)
  84   {
  85                 PM_RAISE(retval, PM_RET_EX_TYPE);
  86                 return retval;
  87         }
  88
  89         // Get lastWakeTimeMs argument
  90         pobj = NATIVE_GET_LOCAL(0);
  91         if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_INT )
  92                 lastWakeTimeTicks = (portTickType)(((pPmInt_t) pobj)->val)  / portTICK_RATE_MS;
  93         else if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_FLT )
  94                 lastWakeTimeTicks = (portTickType)(((pPmFloat_t) pobj)->val)  / portTICK_RATE_MS;
  95         else
  96         {
  97                 PM_RAISE(retval, PM_RET_EX_TYPE);
  98                 return retval;
  99         }
 100
 101         // Get timeToDelayMs argument
 102         pobj = NATIVE_GET_LOCAL(1);
 103         if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_INT )
 104                 timeToDelayTicks = (portTickType)(((pPmInt_t) pobj)->val)  / portTICK_RATE_MS;
 105         else if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_FLT )
 106                 timeToDelayTicks = (portTickType)(((pPmFloat_t) pobj)->val)  / portTICK_RATE_MS;
 107         else
 108         {
 109                 PM_RAISE(retval, PM_RET_EX_TYPE);
 110                 return retval;
 111         }
 112
 113         // Delay
 114         vTaskDelayUntil(&lastWakeTimeTicks, timeToDelayTicks);
 115
 116   // Return an int object with the time value */
 117         retval = int_new((int32_t)(lastWakeTimeTicks*portTICK_RATE_MS), &pobjret);
 118   NATIVE_SET_TOS(pobjret);
 119   return retval;
 120         """
 121         pass
 122
 123 # Update FlightPlanStatus debug fields
 124 def debug(val1, val2):
 125         """__NATIVE__
 126         pPmObj_t pobj;
 127         FlightPlanStatusData status;
 128         PmReturn_t retval;
 129
 130         // Check number of arguments
 131         if (NATIVE_GET_NUM_ARGS() != 2)
 132         {
 133                 PM_RAISE(retval, PM_RET_EX_TYPE);
 134                 return retval;
 135         }
 136
 137         // Get status object
 138         FlightPlanStatusGet(&status);
 139
 140         // Update debug1
 141         pobj = NATIVE_GET_LOCAL(0);
 142         if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_INT )
 143                 status.Debug[0] = (float)(((pPmInt_t) pobj)->val);
 144         else if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_FLT )
 145                 status.Debug[0] = (float)(((pPmFloat_t) pobj)->val);
 146         else
 147         {
 148                 PM_RAISE(retval, PM_RET_EX_TYPE);
 149                 return retval;
 150         }
 151
 152         // Update debug2
 153         pobj = NATIVE_GET_LOCAL(1);
 154         if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_INT )
 155                 status.Debug[1] = (float)(((pPmInt_t) pobj)->val);
 156         else if ( OBJ_GET_TYPE(pobj) == OBJ_TYPE_FLT )
 157                 status.Debug[1] = (float)(((pPmFloat_t) pobj)->val);
 158         else
 159         {
 160                 PM_RAISE(retval, PM_RET_EX_TYPE);
 161                 return retval;
 162         }
 163
 164         // Update status object
 165         FlightPlanStatusSet(&status);
 166
 167         return PM_RET_OK;
 168         """
 169         pass
 170
 171 # Returns 1 if a stop request is pending. The script should periodically check
 172 # for stop requests and exit using sys.exit() if one is detected.
 173 def hasStopRequest():
 174         """__NATIVE__
 175         pPmObj_t pobjret;
 176         PmReturn_t retval;
 177         FlightPlanControlData control;
 178         uint32_t stopRequest;
 179
 180         // Get control object
 181         FlightPlanControlGet(&control);
 182
 183         // Check if a stop request is pending
 184         if (control.Command == FLIGHTPLANCONTROL_COMMAND_STOP)
 185                 stopRequest = 1;
 186         else
 187                 stopRequest = 0;
 188
 189         // Return
 190         retval = int_new((int32_t)(stopRequest), &pobjret);
 191   NATIVE_SET_TOS(pobjret);
 192   return retval;
 193         """
 194         pass
 195
 196 # TODO: Wait for object updates in the event queue
 197 def waitForObjectUpdates(timeoutMs):
 198         pass
 199
 200
 201
 202