flight/modules/FlightPlan/lib/uavobject.py

   1 ##
   2 ##############################################################################
   3 #
   4 # @file       uavobject.py
   5 # @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
   6 # @brief      Base classes for python UAVObject
   7 #
   8 # @see        The GNU Public License (GPL) Version 3
   9 #
  10 #############################################################################/
  11 #
  12 # This program is free software; you can redistribute it and/or modify
  13 # it under the terms of the GNU General Public License as published by
  14 # the Free Software Foundation; either version 3 of the License, or
  15 # (at your option) any later version.
  16 #
  17 # This program is distributed in the hope that it will be useful, but
  18 # WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  19 # or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  20 # for more details.
  21 #
  22 # You should have received a copy of the GNU General Public License along
  23 # with this program; if not, write to the Free Software Foundation, Inc.,
  24 # 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  25 #
  26
  27 """__NATIVE__
  28 #include "openpilot.h"
  29
  30 #define TYPE_INT8 0
  31 #define TYPE_INT16 1
  32 #define TYPE_INT32 2
  33 #define TYPE_UINT8 3
  34 #define TYPE_UINT16 4
  35 #define TYPE_UINT32 5
  36 #define TYPE_FLOAT32 6
  37 #define TYPE_ENUM 7
  38
  39 """
  40
  41 from list import append
  42
  43 class UAVObjectMetadata:
  44         class UpdateMode:
  45                 PERIODIC = 0
  46                 ONCHANGE = 1
  47                 MANUAL = 2
  48                 NEVER = 3
  49
  50         class Access:
  51                 READWRITE = 0
  52                 READONLY = 1
  53
  54         def __init__(self, objId):
  55                 self.access = UAVObjectMetadata.Access.READWRITE
  56                 self.gcsAccess = UAVObjectMetadata.Access.READWRITE
  57                 self.telemetryAcked = False
  58                 self.telemetryUpdateMode = UAVObjectMetadata.UpdateMode.MANUAL
  59                 self.telemetryUpdatePeriod = 0
  60                 self.gcsTelemetryAcked = False
  61                 self.gcsTelemetryUpdateMode = UAVObjectMetadata.UpdateMode.MANUAL
  62                 self.gcsTelemetryUpdatePeriod = 0
  63                 self.loggingUpdateMode = 0
  64                 self.loggingUpdatePeriod = UAVObjectMetadata.UpdateMode.MANUAL
  65                 self.objId = objId
  66                 self.read()
  67
  68         def read(self):
  69                 pass
  70
  71         def write(self):
  72                 pass
  73
  74 class UAVObjectField:
  75         class FType:
  76                 INT8 = 0
  77                 INT16 = 1
  78                 INT32 = 2
  79                 UINT8 = 3
  80                 UINT16 = 4
  81                 UINT32 = 5
  82                 FLOAT32 = 6
  83                 ENUM = 7
  84
  85         def __init__(self, ftype, numElements):
  86                 self.ftype = ftype
  87                 self.numElements = numElements
  88                 if ftype == UAVObjectField.FType.FLOAT32:
  89                         if numElements == 1:
  90                                 self.value = 0.0
  91                         else:
  92                                 self.value = []
  93                                 for n in range(0, numElements):
  94                                         append(self.value, 0.0)
  95                 else:
  96                         if numElements == 1:
  97                                 self.value = 0
  98                         else:
  99                                 self.value = []
 100                                 for n in range(0, numElements):
 101                                         append(self.value, 0)
 102
 103 class UAVObject:
 104         def __init__(self, objId):
 105                 self.metadata = UAVObjectMetadata(objId)
 106                 self.objId = objId
 107                 self.instId = 0
 108                 self.fields = []
 109
 110         def addField(self, field):
 111                 append(self.fields, field)
 112         '''
 113         #
 114         # Support for getName was removed from embedded UAVO database to save RAM + Flash
 115         #
 116         def getName(self):
 117                 """__NATIVE__
 118                 UAVObjHandle objHandle;
 119                 pPmObj_t nameObj;
 120                 pPmObj_t self;
 121                 pPmObj_t attrs;
 122                 pPmObj_t fieldName;
 123                 pPmObj_t field;
 124                 PmReturn_t retval;
 125                 uint32_t objId;
 126                 const char* name;
 127                 uint8_t const *tmpStr;
 128
 129                 // Get dictionary of class attributes
 130                 self = NATIVE_GET_LOCAL(0);
 131                 attrs = (pPmObj_t)((pPmInstance_t)self)->cli_attrs;
 132
 133                 // Get object ID
 134                 tmpStr = (uint8_t const *)"objId";
 135                 retval = string_new(&tmpStr, &fieldName); PM_RETURN_IF_ERROR(retval);
 136                 retval = dict_getItem(attrs, fieldName, &field); PM_RETURN_IF_ERROR(retval);
 137                 objId = ((pPmInt_t) field)->val;
 138
 139                 // Get name
 140                 objHandle = UAVObjGetByID(objId);
 141                 name = UAVObjGetName(objHandle);
 142
 143                 // Create return object
 144                 retval = string_new(name, &nameObj); PM_RETURN_IF_ERROR(retval);
 145                 NATIVE_SET_TOS(nameObj);
 146                 return PM_RET_OK;
 147                 """
 148                 pass
 149         '''
 150
 151         def read(self):
 152                 """__NATIVE__
 153                 uint8_t numBytes;
 154                 UAVObjHandle objHandle;
 155                 uint32_t objId;
 156                 uint16_t instId;
 157                 pPmObj_t self;
 158                 pPmObj_t attrs;
 159                 pPmObj_t field;
 160                 pPmObj_t fields;
 161                 pPmObj_t fieldName;
 162                 pPmObj_t value;
 163                 PmReturn_t retval;
 164                 uint32_t numFields;
 165                 uint32_t fieldIdx;
 166                 uint32_t dataIdx;
 167                 uint32_t valueIdx;
 168                 uint32_t type;
 169                 uint32_t numElements;
 170                 uint8_t const *tmpStr;
 171                 int16_t *tmpInt16;
 172                 int32_t *tmpInt32;
 173                 float *tmpFloat;
 174
 175                 // Get dictionary of class attributes
 176                 self = NATIVE_GET_LOCAL(0);
 177                 attrs = (pPmObj_t)((pPmInstance_t)self)->cli_attrs;
 178
 179                 // Get object ID
 180                 tmpStr = (uint8_t const *)"objId";
 181                 retval = string_new(&tmpStr, &fieldName); PM_RETURN_IF_ERROR(retval);
 182                 retval = dict_getItem(attrs, fieldName, &field); PM_RETURN_IF_ERROR(retval);
 183                 objId = ((pPmInt_t) field)->val;
 184
 185                 // Get the instance ID
 186                 tmpStr = (uint8_t const *)"instId";
 187                 retval = string_new(&tmpStr, &fieldName); PM_RETURN_IF_ERROR(retval);
 188                 retval = dict_getItem(attrs, fieldName, &field); PM_RETURN_IF_ERROR(retval);
 189                 instId = ((pPmInt_t) field)->val;
 190
 191                 // Get handle and number of bytes in the object
 192                 objHandle = UAVObjGetByID(objId);
 193                 numBytes = UAVObjGetNumBytes(objHandle);
 194                 uint8_t data[numBytes];
 195
 196                 // Read object data
 197                 UAVObjGetInstanceData(objHandle, instId, data);
 198
 199                 // Get dictionary of fields
 200                 tmpStr = (uint8_t const *)"fields";
 201                 retval = string_new(&tmpStr, &fieldName); PM_RETURN_IF_ERROR(retval);
 202                 retval = dict_getItem(attrs, fieldName, &fields); PM_RETURN_IF_ERROR(retval);
 203                 numFields = ((pPmList_t) fields)->length;
 204
 205                 // Process each field
 206                 dataIdx = 0;
 207                 for (fieldIdx = 0; fieldIdx < numFields; ++fieldIdx)
 208                 {
 209                         // Get field
 210                         retval = list_getItem(fields, fieldIdx, &field); PM_RETURN_IF_ERROR(retval);
 211                         attrs = (pPmObj_t)((pPmInstance_t)field)->cli_attrs;
 212                         // Get type
 213                         tmpStr = (uint8_t const *)"ftype";
 214                         retval = string_new(&tmpStr, &fieldName); PM_RETURN_IF_ERROR(retval);
 215                         retval = dict_getItem(attrs, fieldName, &field); PM_RETURN_IF_ERROR(retval);
 216                         type = ((pPmInt_t) field)->val;
 217                         // Get number of elements
 218                         tmpStr = (uint8_t const *)"numElements";
 219                         retval = string_new(&tmpStr, &fieldName); PM_RETURN_IF_ERROR(retval);
 220                         retval = dict_getItem(attrs, fieldName, &field); PM_RETURN_IF_ERROR(retval);
 221                         numElements = ((pPmInt_t) field)->val;
 222                         // Get value
 223                         tmpStr = (uint8_t const *)"value";
 224                         retval = string_new(&tmpStr, &fieldName); PM_RETURN_IF_ERROR(retval);
 225                         retval = dict_getItem(attrs, fieldName, &field); PM_RETURN_IF_ERROR(retval);
 226                         // Set value for each element
 227                         for (valueIdx = 0; valueIdx < numElements; ++valueIdx)
 228                         {
 229                                 // Update value based on type
 230                                 switch (type)
 231                                 {
 232                                         case TYPE_INT8:
 233                                         case TYPE_UINT8:
 234                                         case TYPE_ENUM:
 235                                                 retval = int_new(data[dataIdx], &value); PM_RETURN_IF_ERROR(retval);
 236                                                 dataIdx = dataIdx + 1;
 237                                                 break;
 238                                         case TYPE_INT16:
 239                                         case TYPE_UINT16:
 240                                                 tmpInt16 = (int16_t*)(&data[dataIdx]);
 241                                                 retval = int_new(*tmpInt16, &value); PM_RETURN_IF_ERROR(retval);
 242                                                 dataIdx = dataIdx + 2;
 243                                                 break;
 244                                         case TYPE_INT32:
 245                                         case TYPE_UINT32:
 246                                                 tmpInt32 = (int32_t*)(&data[dataIdx]);
 247                                                 retval = int_new(*tmpInt32, &value); PM_RETURN_IF_ERROR(retval);
 248                                                 dataIdx = dataIdx + 4;
 249                                                 break;
 250                                         case TYPE_FLOAT32:
 251                                                 tmpFloat = (float*)(&data[dataIdx]);
 252                                                 retval = float_new(*tmpFloat, &value); PM_RETURN_IF_ERROR(retval);
 253                                                 dataIdx = dataIdx + 4;
 254                                                 break;
 255                                 }
 256                                 // Set value
 257                                 if ( OBJ_GET_TYPE(field) == OBJ_TYPE_LST )
 258                                 {
 259                                         retval = list_setItem(field, valueIdx, value); PM_RETURN_IF_ERROR(retval);
 260                                 }
 261                                 else
 262                                 {
 263                                         tmpStr = (uint8_t const *)"value";
 264                                         retval = string_new(&tmpStr, &fieldName); PM_RETURN_IF_ERROR(retval);
 265                                         retval = dict_setItem(attrs, fieldName, value); PM_RETURN_IF_ERROR(retval);
 266                                 }
 267                         }
 268                 }
 269
 270                 // Done
 271                 return PM_RET_OK;
 272                 """
 273                 pass
 274
 275         def write(self):
 276                 """__NATIVE__
 277                 uint8_t numBytes;
 278                 UAVObjHandle objHandle;
 279                 uint32_t objId;
 280                 uint16_t instId;
 281                 pPmObj_t self;
 282                 pPmObj_t attrs;
 283                 pPmObj_t field;
 284                 pPmObj_t fields;
 285                 pPmObj_t fieldName;
 286                 pPmObj_t value;
 287                 PmReturn_t retval;
 288                 uint32_t numFields;
 289                 uint32_t fieldIdx;
 290                 uint32_t dataIdx;
 291                 uint32_t valueIdx;
 292                 uint32_t type;
 293                 uint32_t numElements;
 294                 uint8_t const *tmpStr;
 295                 int8_t tmpInt8 = 0;
 296                 int16_t tmpInt16;
 297                 int32_t tmpInt32;
 298                 float tmpFloat;
 299
 300                 // Get dictionary of class attributes
 301                 self = NATIVE_GET_LOCAL(0);
 302                 attrs = (pPmObj_t)((pPmInstance_t)self)->cli_attrs;
 303
 304                 // Get object ID
 305                 tmpStr = (uint8_t const *)"objId";
 306                 retval = string_new(&tmpStr, &fieldName); PM_RETURN_IF_ERROR(retval);
 307                 retval = dict_getItem(attrs, fieldName, &field); PM_RETURN_IF_ERROR(retval);
 308                 objId = ((pPmInt_t) field)->val;
 309
 310                 // Get the instance ID
 311                 tmpStr = (uint8_t const *)"instId";
 312                 retval = string_new(&tmpStr, &fieldName); PM_RETURN_IF_ERROR(retval);
 313                 retval = dict_getItem(attrs, fieldName, &field); PM_RETURN_IF_ERROR(retval);
 314                 instId = ((pPmInt_t) field)->val;
 315
 316                 // Get handle and number of bytes in the object
 317                 objHandle = UAVObjGetByID(objId);
 318                 numBytes = UAVObjGetNumBytes(objHandle);
 319                 uint8_t data[numBytes];
 320
 321                 // Get dictionary of fields
 322                 tmpStr = (uint8_t const *)"fields";
 323                 retval = string_new(&tmpStr, &fieldName); PM_RETURN_IF_ERROR(retval);
 324                 retval = dict_getItem(attrs, fieldName, &fields); PM_RETURN_IF_ERROR(retval);
 325                 numFields = ((pPmList_t) fields)->length;
 326
 327                 // Process each field
 328                 dataIdx = 0;
 329                 for (fieldIdx = 0; fieldIdx < numFields; ++fieldIdx)
 330                 {
 331                         // Get field
 332                         retval = list_getItem(fields, fieldIdx, &field); PM_RETURN_IF_ERROR(retval);
 333                         attrs = (pPmObj_t)((pPmInstance_t)field)->cli_attrs;
 334                         // Get type
 335                         tmpStr = (uint8_t const *)"ftype";
 336                         retval = string_new(&tmpStr, &fieldName); PM_RETURN_IF_ERROR(retval);
 337                         retval = dict_getItem(attrs, fieldName, &field); PM_RETURN_IF_ERROR(retval);
 338                         type = ((pPmInt_t) field)->val;
 339                         // Get number of elements
 340                         tmpStr = (uint8_t const *)"numElements";
 341                         retval = string_new(&tmpStr, &fieldName); PM_RETURN_IF_ERROR(retval);
 342                         retval = dict_getItem(attrs, fieldName, &field); PM_RETURN_IF_ERROR(retval);
 343                         numElements = ((pPmInt_t) field)->val;
 344                         // Get value
 345                         tmpStr = (uint8_t const *)"value";
 346                         retval = string_new(&tmpStr, &fieldName); PM_RETURN_IF_ERROR(retval);
 347                         retval = dict_getItem(attrs, fieldName, &field); PM_RETURN_IF_ERROR(retval);
 348                         // Set value for each element
 349                         for (valueIdx = 0; valueIdx < numElements; ++valueIdx)
 350                         {
 351                                 // Get value
 352                                 if ( OBJ_GET_TYPE(field) == OBJ_TYPE_LST )
 353                                 {
 354                                         retval = list_getItem(field, valueIdx, &value); PM_RETURN_IF_ERROR(retval);
 355                                 }
 356                                 else
 357                                         value = field;
 358                                 // Update value based on type
 359                                 switch (type)
 360                                 {
 361                                         case TYPE_INT8:
 362                                         case TYPE_UINT8:
 363                                         case TYPE_ENUM:
 364                                                 if ( OBJ_GET_TYPE(value) == OBJ_TYPE_INT )
 365                                                 {
 366                                                         tmpInt8 = (int8_t)((pPmInt_t)value)->val;
 367                                                 }
 368                                                 else if ( OBJ_GET_TYPE(value) == OBJ_TYPE_FLT )
 369                                                 {
 370                                                     tmpInt8 = (int8_t)((pPmFloat_t)value)->val;
 371                                                 }
 372                                                 memcpy( &data[dataIdx], &tmpInt8, 1 );
 373                                                 dataIdx = dataIdx + 1;
 374                                                 break;
 375                                         case TYPE_INT16:
 376                                         case TYPE_UINT16:
 377                                                 if ( OBJ_GET_TYPE(value) == OBJ_TYPE_INT )
 378                                                 {
 379                                                         tmpInt16 = (int16_t)((pPmInt_t)value)->val;
 380                                                 }
 381                                                 else if ( OBJ_GET_TYPE(value) == OBJ_TYPE_FLT )
 382                                                 {
 383                                                     tmpInt16 = (int16_t)((pPmFloat_t)value)->val;
 384                                                 }
 385                                                 memcpy( &data[dataIdx], &tmpInt16, 2 );
 386                                                 dataIdx = dataIdx + 2;
 387                                                 break;
 388                                         case TYPE_INT32:
 389                                         case TYPE_UINT32:
 390                                                 if ( OBJ_GET_TYPE(value) == OBJ_TYPE_INT )
 391                                                 {
 392                                                         tmpInt32 = (int32_t)((pPmInt_t)value)->val;
 393                                                 }
 394                                                 else if ( OBJ_GET_TYPE(value) == OBJ_TYPE_FLT )
 395                                                 {
 396                                                     tmpInt32 = (int32_t)((pPmFloat_t)value)->val;
 397                                                 }
 398                                                 memcpy( &data[dataIdx], &tmpInt32, 4 );
 399                                                 dataIdx = dataIdx + 4;
 400                                                 break;
 401                                         case TYPE_FLOAT32:
 402                                                 if ( OBJ_GET_TYPE(value) == OBJ_TYPE_INT )
 403                                                 {
 404                                                         tmpFloat = (float)((pPmInt_t)value)->val;
 405                                                 }
 406                                                 else if ( OBJ_GET_TYPE(value) == OBJ_TYPE_FLT )
 407                                                 {
 408                                                     tmpFloat = (float)((pPmFloat_t)value)->val;
 409                                                 }
 410                                                 memcpy( &data[dataIdx], &tmpFloat, 4 );
 411                                                 dataIdx = dataIdx + 4;
 412                                                 break;
 413                                 }
 414                         }
 415                 }
 416
 417                 // Write object data
 418                 UAVObjSetInstanceData(objHandle, instId, data);
 419
 420                 // Done
 421                 return PM_RET_OK;
 422                 """
 423                 pass
 424
 425
 426
 427
 428
 429