flight/uavobjects/uavobjectmanager.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup UAVObjects OpenPilot UAVObjects
   4  * @{
   5  * @addtogroup UAV Object Manager
   6  * @brief The core UAV Objects functions, most of which are wrappered by
   7  * autogenerated defines
   8  * @{
   9  *
  10  *
  11  * @file       uavobjectmanager.h
  12  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  13  * @brief      Object manager library. This library holds a collection of all objects.
  14  *             It can be used by all modules/libraries to find an object reference.
  15  * @see        The GNU Public License (GPL) Version 3
  16  *
  17  *****************************************************************************/
  18 /*
  19  * This program is free software; you can redistribute it and/or modify
  20  * it under the terms of the GNU General Public License as published by
  21  * the Free Software Foundation; either version 3 of the License, or
  22  * (at your option) any later version.
  23  *
  24  * This program is distributed in the hope that it will be useful, but
  25  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  26  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  27  * for more details.
  28  *
  29  * You should have received a copy of the GNU General Public License along
  30  * with this program; if not, write to the Free Software Foundation, Inc.,
  31  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  32  */
  33
  34 #include "openpilot.h"
  35 #include "pios_struct_helper.h"
  36
  37 extern uintptr_t pios_uavo_settings_fs_id;
  38
  39 #if (defined(__MACH__) && defined(__APPLE__))
  40 #include <mach-o/getsect.h>
  41 #endif
  42
  43 // Constants
  44
  45 // Private types
  46
  47 // Macros
  48 #define SET_BITS(var, shift, value, mask) var = (var & ~(mask << shift)) | (value << shift);
  49
  50 // Mach-o: dummy segment to calculate ASLR offset in sim_osx
  51 #if (defined(__MACH__) && defined(__APPLE__))
  52 static long _aslr_offset __attribute__((section("__DATA,_aslr")));
  53 #endif
  54
  55 /* Table of UAVO handles */
  56 #if (defined(__MACH__) && defined(__APPLE__))
  57 /* Mach-o format */
  58 static struct UAVOData * *__start__uavo_handles;
  59 static struct UAVOData * *__stop__uavo_handles;
  60 #else
  61 /* ELF format: automagically defined at compile time */
  62 extern struct UAVOData *__start__uavo_handles[] __attribute__((weak));
  63 extern struct UAVOData *__stop__uavo_handles[] __attribute__((weak));
  64 #endif
  65
  66 #define UAVO_LIST_ITERATE(_item) \
  67     for (struct UAVOData * *_uavo_slot = __start__uavo_handles; \
  68          _uavo_slot && _uavo_slot < __stop__uavo_handles; \
  69          _uavo_slot++) { \
  70         struct UAVOData *_item = *_uavo_slot; \
  71         if (_item == NULL) { continue; }
  72
  73 /**
  74  * List of event queues and the eventmask associated with the queue.
  75  */
  76
  77 /** opaque type for instances **/
  78 typedef void *InstanceHandle;
  79
  80 struct ObjectEventEntry {
  81     struct ObjectEventEntry *next;
  82     xQueueHandle queue;
  83     UAVObjEventCallback     cb;
  84     uint8_t eventMask;
  85 };
  86
  87 /*
  88    MetaInstance   == [UAVOBase [UAVObjMetadata]]
  89    SingleInstance == [UAVOBase [UAVOData [InstanceData]]]
  90    MultiInstance  == [UAVOBase [UAVOData [NumInstances [InstanceData0 [next]]]]
  91                                                   ____________________/
  92    \-->[InstanceData1 [next]]
  93                                                   _________...________/
  94    \-->[InstanceDataN [next]]
  95  */
  96
  97 /*
  98  * UAVO Base Type
  99  *   - All Types of UAVObjects are of this base type
 100  *   - The flags determine what type(s) this object
 101  */
 102 struct UAVOBase {
 103     /* Let these objects be added to an event queue */
 104     struct ObjectEventEntry *next_event;
 105
 106     /* Describe the type of object that follows this header */
 107     struct UAVOInfo {
 108         bool isMeta        : 1;
 109         bool isSingle      : 1;
 110         bool isSettings    : 1;
 111         bool isPriority    : 1;
 112     } flags;
 113 } __attribute__((packed));
 114
 115 /* Augmented type for Meta UAVO */
 116 struct UAVOMeta {
 117     struct UAVOBase base;
 118     UAVObjMetadata  instance0;
 119 } __attribute__((packed));
 120
 121 /* Shared data structure for all data-carrying UAVObjects (UAVOSingle and UAVOMulti) */
 122 struct UAVOData {
 123     struct UAVOBase base;
 124     uint32_t id;
 125     /*
 126      * Embed the Meta object as another complete UAVO
 127      * inside the payload for this UAVO.
 128      */
 129     struct UAVOMeta metaObj;
 130     uint16_t instance_size;
 131 } __attribute__((packed, aligned(4)));
 132
 133 /* Augmented type for Single Instance Data UAVO */
 134 struct UAVOSingle {
 135     struct UAVOData uavo;
 136
 137     uint8_t instance0[];
 138     /*
 139      * Additional space will be malloc'd here to hold the
 140      * the data for this instance.
 141      */
 142 } __attribute__((packed));
 143
 144 /* Part of a linked list of instances chained off of a multi instance UAVO. */
 145 struct UAVOMultiInst {
 146     struct UAVOMultiInst *next;
 147     uint8_t instance[];
 148     /*
 149      * Additional space will be malloc'd here to hold the
 150      * the data for this instance.
 151      */
 152 } __attribute__((packed));
 153
 154 /* Augmented type for Multi Instance Data UAVO */
 155 struct UAVOMulti {
 156     struct UAVOData uavo;
 157     uint16_t num_instances;
 158     struct UAVOMultiInst instance0 __attribute__((aligned(4)));
 159     /*
 160      * Additional space will be malloc'd here to hold the
 161      * the data for instance 0.
 162      */
 163 } __attribute__((packed));
 164
 165 /** all information about a metaobject are hardcoded constants **/
 166 #define MetaNumBytes sizeof(UAVObjMetadata)
 167 #define MetaBaseObjectPtr(obj)           ((struct UAVOData *)((obj) - offsetof(struct UAVOData, metaObj)))
 168 #define MetaObjectPtr(obj)               ((struct UAVODataMeta *)&((obj)->metaObj))
 169 #define MetaDataPtr(obj)                 ((UAVObjMetadata *)&((obj)->instance0))
 170 #define LinkedMetaDataPtr(obj)           ((UAVObjMetadata *)&((obj)->metaObj.instance0))
 171
 172 /** all information about instances are dependant on object type **/
 173 #define ObjSingleInstanceDataOffset(obj) ((void *)(&(((struct UAVOSingle *)obj)->instance0)))
 174 #define InstanceDataOffset(inst)         ((void *)&(((struct UAVOMultiInst *)inst)->instance))
 175 #define InstanceData(instance)           ((void *)instance)
 176
 177 // Private functions
 178 static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId, UAVObjEventType event);
 179 static InstanceHandle createInstance(struct UAVOData *obj, uint16_t instId);
 180 static InstanceHandle getInstance(struct UAVOData *obj, uint16_t instId);
 181 static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue, UAVObjEventCallback cb, uint8_t eventMask);
 182 static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue, UAVObjEventCallback cb);
 183 static void instanceAutoUpdated(UAVObjHandle obj_handle, uint16_t instId);
 184
 185 // Private variables
 186 static xSemaphoreHandle mutex;
 187 static const UAVObjMetadata defMetadata = {
 188     .flags                    = (ACCESS_READWRITE << UAVOBJ_ACCESS_SHIFT |
 189               ACCESS_READWRITE << UAVOBJ_GCS_ACCESS_SHIFT |
 190               1 << UAVOBJ_TELEMETRY_ACKED_SHIFT |
 191               1 << UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT |
 192               UPDATEMODE_ONCHANGE << UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT |
 193               UPDATEMODE_ONCHANGE << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT),
 194     .telemetryUpdatePeriod    = 0,
 195     .gcsTelemetryUpdatePeriod = 0,
 196     .loggingUpdatePeriod      = 0,
 197 };
 198
 199 static UAVObjStats stats;
 200
 201 /**
 202  * Initialize the object manager
 203  * \return 0 Success
 204  * \return -1 Failure
 205  */
 206 int32_t UAVObjInitialize()
 207 {
 208     // Initialize variables
 209     memset(&stats, 0, sizeof(UAVObjStats));
 210
 211     /* Initialize _uavo_handles start/stop pointers */
 212         #if (defined(__MACH__) && defined(__APPLE__))
 213     uint64_t aslr_offset = (uint64_t)&_aslr_offset - getsectbyname("__DATA", "_aslr")->addr;
 214     __start__uavo_handles = (struct UAVOData * *)(getsectbyname("__DATA", "_uavo_handles")->addr + aslr_offset);
 215     __stop__uavo_handles  = (struct UAVOData * *)((uint64_t)__start__uavo_handles + getsectbyname("__DATA", "_uavo_handles")->size);
 216         #endif
 217
 218     // Initialize the uavo handle table
 219     memset(__start__uavo_handles, 0,
 220            (uintptr_t)__stop__uavo_handles - (uintptr_t)__start__uavo_handles);
 221
 222     // Create mutex
 223     mutex = xSemaphoreCreateRecursiveMutex();
 224     if (mutex == NULL) {
 225         return -1;
 226     }
 227
 228     // Done
 229     return 0;
 230 }
 231
 232 /*****************
 233  * Statistics
 234  ****************/
 235
 236 /**
 237  * Get the statistics counters
 238  * @param[out] statsOut The statistics counters will be copied there
 239  */
 240 void UAVObjGetStats(UAVObjStats *statsOut)
 241 {
 242     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 243     memcpy(statsOut, &stats, sizeof(UAVObjStats));
 244     xSemaphoreGiveRecursive(mutex);
 245 }
 246
 247 /**
 248  * Clear the statistics counters
 249  */
 250 void UAVObjClearStats()
 251 {
 252     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 253     memset(&stats, 0, sizeof(UAVObjStats));
 254     xSemaphoreGiveRecursive(mutex);
 255 }
 256
 257 /************************
 258  * Object Initialization
 259  ***********************/
 260
 261 static void UAVObjInitMetaData(struct UAVOMeta *obj_meta)
 262 {
 263     /* Fill in the common part of the UAVO */
 264     struct UAVOBase *uavo_base = &(obj_meta->base);
 265
 266     memset(uavo_base, 0, sizeof(*uavo_base));
 267     uavo_base->flags.isMeta   = true;
 268     uavo_base->flags.isSingle = true;
 269     uavo_base->next_event     = NULL;
 270
 271     /* Clear the instance data carried in the UAVO */
 272     memset(&(obj_meta->instance0), 0, sizeof(obj_meta->instance0));
 273 }
 274
 275 static struct UAVOData *UAVObjAllocSingle(uint32_t num_bytes)
 276 {
 277     /* Compute the complete size of the object, including the data for a single embedded instance */
 278     uint32_t object_size = sizeof(struct UAVOSingle) + num_bytes;
 279
 280     /* Allocate the object from the heap */
 281     struct UAVOSingle *uavo_single = (struct UAVOSingle *)pios_malloc(object_size);
 282
 283     if (!uavo_single) {
 284         return NULL;
 285     }
 286
 287     /* Fill in the common part of the UAVO */
 288     struct UAVOBase *uavo_base = &(uavo_single->uavo.base);
 289     memset(uavo_base, 0, sizeof(*uavo_base));
 290     uavo_base->flags.isSingle = true;
 291     uavo_base->next_event     = NULL;
 292
 293     /* Clear the instance data carried in the UAVO */
 294     memset(&(uavo_single->instance0), 0, num_bytes);
 295
 296     /* Give back the generic UAVO part */
 297     return &(uavo_single->uavo);
 298 }
 299
 300 static struct UAVOData *UAVObjAllocMulti(uint32_t num_bytes)
 301 {
 302     /* Compute the complete size of the object, including the data for a single embedded instance */
 303     uint32_t object_size = sizeof(struct UAVOMulti) + num_bytes;
 304
 305     /* Allocate the object from the heap */
 306     struct UAVOMulti *uavo_multi = (struct UAVOMulti *)pios_malloc(object_size);
 307
 308     if (!uavo_multi) {
 309         return NULL;
 310     }
 311
 312     /* Fill in the common part of the UAVO */
 313     struct UAVOBase *uavo_base = &(uavo_multi->uavo.base);
 314     memset(uavo_base, 0, sizeof(*uavo_base));
 315     uavo_base->flags.isSingle = false;
 316     uavo_base->next_event     = NULL;
 317
 318     /* Set up the type-specific part of the UAVO */
 319     uavo_multi->num_instances = 1;
 320
 321     /* Clear the multi instance data carried in the UAVO */
 322     memset(&(uavo_multi->instance0), 0, sizeof(struct UAVOMultiInst) + num_bytes);
 323
 324     /* Give back the generic UAVO part */
 325     return &(uavo_multi->uavo);
 326 }
 327
 328 /**************************
 329  * UAVObject Database APIs
 330  *************************/
 331
 332 /**
 333  * Register and new object in the object manager.
 334  * \param[in] id Unique object ID
 335  * \param[in] isSingleInstance Is this a single instance or multi-instance object
 336  * \param[in] isSettings Is this a settings object
 337  * \param[in] numBytes Number of bytes of object data (for one instance)
 338  * \param[in] initCb Default field and metadata initialization function
 339  * \return Object handle, or NULL if failure.
 340  * \return
 341  */
 342 UAVObjHandle UAVObjRegister(uint32_t id,
 343                             bool isSingleInstance, bool isSettings, bool isPriority,
 344                             uint32_t num_bytes,
 345                             UAVObjInitializeCallback initCb)
 346 {
 347     struct UAVOData *uavo_data = NULL;
 348
 349     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 350
 351     /* Don't allow duplicate registrations */
 352     if (UAVObjGetByID(id)) {
 353         goto unlock_exit;
 354     }
 355
 356     /* Map the various flags to one of the UAVO types we understand */
 357     if (isSingleInstance) {
 358         uavo_data = UAVObjAllocSingle(num_bytes);
 359     } else {
 360         uavo_data = UAVObjAllocMulti(num_bytes);
 361     }
 362
 363     if (!uavo_data) {
 364         goto unlock_exit;
 365     }
 366
 367     /* Fill in the details about this UAVO */
 368     uavo_data->id = id;
 369     uavo_data->instance_size = num_bytes;
 370     if (isSettings) {
 371         uavo_data->base.flags.isSettings = true;
 372         // settings defaults to being sent with priority
 373         uavo_data->base.flags.isPriority = true;
 374     } else {
 375         uavo_data->base.flags.isPriority = isPriority;
 376     }
 377     /* Initialize the embedded meta UAVO */
 378     UAVObjInitMetaData(&uavo_data->metaObj);
 379
 380     /* Initialize object fields and metadata to default values */
 381     if (initCb) {
 382         initCb((UAVObjHandle)uavo_data, 0);
 383     }
 384
 385     /* Always try to load the meta object from flash */
 386     UAVObjLoad((UAVObjHandle) & (uavo_data->metaObj), 0);
 387
 388     /* Attempt to load settings object from flash */
 389     if (uavo_data->base.flags.isSettings) {
 390         UAVObjLoad((UAVObjHandle)uavo_data, 0);
 391     }
 392
 393     // fire events for outer object and its embedded meta object
 394     instanceAutoUpdated((UAVObjHandle)uavo_data, 0);
 395     instanceAutoUpdated((UAVObjHandle) & (uavo_data->metaObj), 0);
 396
 397 unlock_exit:
 398     xSemaphoreGiveRecursive(mutex);
 399     return (UAVObjHandle)uavo_data;
 400 }
 401
 402 /**
 403  * Retrieve an object from the list given its id
 404  * \param[in] The object ID
 405  * \return The object or NULL if not found.
 406  */
 407 UAVObjHandle UAVObjGetByID(uint32_t id)
 408 {
 409     UAVObjHandle *found_obj = (UAVObjHandle *)NULL;
 410
 411     // Get lock
 412     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 413
 414     // Look for object
 415     UAVO_LIST_ITERATE(tmp_obj)
 416     if (tmp_obj->id == id) {
 417         found_obj = (UAVObjHandle *)tmp_obj;
 418         goto unlock_exit;
 419     }
 420     if (MetaObjectId(tmp_obj->id) == id) {
 421         found_obj = (UAVObjHandle *)&(tmp_obj->metaObj);
 422         goto unlock_exit;
 423     }
 424 }
 425
 426 unlock_exit:
 427 xSemaphoreGiveRecursive(mutex);
 428 return found_obj;
 429 }
 430
 431 /**
 432  * Get the object's ID
 433  * \param[in] obj The object handle
 434  * \return The object ID
 435  */
 436 uint32_t UAVObjGetID(UAVObjHandle obj_handle)
 437 {
 438     PIOS_Assert(obj_handle);
 439
 440     /* Recover the common object header */
 441     struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle;
 442
 443     if (UAVObjIsMetaobject(obj_handle)) {
 444         /* We have a meta object, find our containing UAVO */
 445         struct UAVOData *uavo_data = container_of((struct UAVOMeta *)uavo_base, struct UAVOData, metaObj);
 446
 447         return MetaObjectId(uavo_data->id);
 448     } else {
 449         /* We have a data object, augment our pointer */
 450         struct UAVOData *uavo_data = (struct UAVOData *)uavo_base;
 451
 452         return uavo_data->id;
 453     }
 454 }
 455
 456 /**
 457  * Get the number of bytes of the object's data (for one instance)
 458  * \param[in] obj The object handle
 459  * \return The number of bytes
 460  */
 461 uint32_t UAVObjGetNumBytes(UAVObjHandle obj)
 462 {
 463     PIOS_Assert(obj);
 464
 465     uint32_t instance_size;
 466
 467     /* Recover the common object header */
 468     struct UAVOBase *uavo_base = (struct UAVOBase *)obj;
 469
 470     if (uavo_base->flags.isMeta) {
 471         instance_size = MetaNumBytes;
 472     } else {
 473         /* We have a data object, augment our pointer */
 474         struct UAVOData *uavo = (struct UAVOData *)uavo_base;
 475
 476         instance_size = uavo->instance_size;
 477     }
 478
 479     return instance_size;
 480 }
 481
 482 /**
 483  * Get the object this object is linked to. For regular objects, the linked object
 484  * is the metaobject. For metaobjects the linked object is the parent object.
 485  * This function is normally only needed by the telemetry module.
 486  * \param[in] obj The object handle
 487  * \return The object linked object handle
 488  */
 489 UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj_handle)
 490 {
 491     PIOS_Assert(obj_handle);
 492
 493     /* Recover the common object header */
 494     struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle;
 495
 496     if (UAVObjIsMetaobject(obj_handle)) {
 497         /* We have a meta object, find our containing UAVO. */
 498         struct UAVOData *uavo_data = container_of((struct UAVOMeta *)uavo_base, struct UAVOData, metaObj);
 499
 500         return (UAVObjHandle)uavo_data;
 501     } else {
 502         /* We have a data object, augment our pointer */
 503         struct UAVOData *uavo_data = (struct UAVOData *)uavo_base;
 504
 505         return (UAVObjHandle) & (uavo_data->metaObj);
 506     }
 507 }
 508
 509 /**
 510  * Get the number of instances contained in the object.
 511  * \param[in] obj The object handle
 512  * \return The number of instances
 513  */
 514 uint16_t UAVObjGetNumInstances(UAVObjHandle obj_handle)
 515 {
 516     PIOS_Assert(obj_handle);
 517
 518     if (UAVObjIsSingleInstance(obj_handle)) {
 519         /* Only one instance is allowed */
 520         return 1;
 521     } else {
 522         /* Multi-instance object.  Inspect the object */
 523         /* Augment our pointer to reflect the proper type */
 524         struct UAVOMulti *uavo_multi = (struct UAVOMulti *)obj_handle;
 525         return uavo_multi->num_instances;
 526     }
 527 }
 528
 529 /**
 530  * Create a new instance in the object.
 531  * \param[in] obj The object handle
 532  * \return The instance ID or 0 if an error
 533  */
 534 uint16_t UAVObjCreateInstance(UAVObjHandle obj_handle,
 535                               UAVObjInitializeCallback initCb)
 536 {
 537     PIOS_Assert(obj_handle);
 538
 539     if (UAVObjIsMetaobject(obj_handle)) {
 540         return 0;
 541     }
 542
 543     // Lock
 544     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 545
 546     InstanceHandle instEntry;
 547     uint16_t instId = 0;
 548
 549     // Create new instance
 550     instId    = UAVObjGetNumInstances(obj_handle);
 551     instEntry = createInstance((struct UAVOData *)obj_handle, instId);
 552     if (instEntry == NULL) {
 553         goto unlock_exit;
 554     }
 555
 556     // Initialize instance data
 557     if (initCb) {
 558         initCb(obj_handle, instId);
 559     }
 560
 561 unlock_exit:
 562     xSemaphoreGiveRecursive(mutex);
 563
 564     return instId;
 565 }
 566
 567 /**
 568  * Does this object contains a single instance or multiple instances?
 569  * \param[in] obj The object handle
 570  * \return True (1) if this is a single instance object
 571  */
 572 bool UAVObjIsSingleInstance(UAVObjHandle obj_handle)
 573 {
 574     PIOS_Assert(obj_handle);
 575
 576     /* Recover the common object header */
 577     struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle;
 578
 579     return uavo_base->flags.isSingle;
 580 }
 581
 582 /**
 583  * Is this a metaobject?
 584  * \param[in] obj The object handle
 585  * \return True (1) if this is metaobject
 586  */
 587 bool UAVObjIsMetaobject(UAVObjHandle obj_handle)
 588 {
 589     PIOS_Assert(obj_handle);
 590
 591     /* Recover the common object header */
 592     struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle;
 593
 594     return uavo_base->flags.isMeta;
 595 }
 596
 597 /**
 598  * Is this a settings object?
 599  * \param[in] obj The object handle
 600  * \return True (1) if this is a settings object
 601  */
 602 bool UAVObjIsSettings(UAVObjHandle obj_handle)
 603 {
 604     PIOS_Assert(obj_handle);
 605
 606     /* Recover the common object header */
 607     struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle;
 608
 609     return uavo_base->flags.isSettings;
 610 }
 611
 612 /**
 613  * Is this a prioritized object?
 614  * \param[in] obj The object handle
 615  * \return True (1) if this is a prioritized object
 616  */
 617 bool UAVObjIsPriority(UAVObjHandle obj_handle)
 618 {
 619     PIOS_Assert(obj_handle);
 620
 621     /* Recover the common object header */
 622     struct UAVOBase *uavo_base = (struct UAVOBase *)obj_handle;
 623
 624     return uavo_base->flags.isPriority;
 625 }
 626
 627
 628 /**
 629  * Unpack an object from a byte array
 630  * \param[in] obj The object handle
 631  * \param[in] instId The instance ID
 632  * \param[in] dataIn The byte array
 633  * \return 0 if success or -1 if failure
 634  */
 635 int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t *dataIn)
 636 {
 637     PIOS_Assert(obj_handle);
 638
 639     // Lock
 640     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 641
 642     int32_t rc = -1;
 643
 644     if (UAVObjIsMetaobject(obj_handle)) {
 645         if (instId != 0) {
 646             goto unlock_exit;
 647         }
 648         memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle), dataIn, MetaNumBytes);
 649     } else {
 650         struct UAVOData *obj;
 651         InstanceHandle instEntry;
 652
 653         // Cast handle to object
 654         obj = (struct UAVOData *)obj_handle;
 655
 656         // Get the instance
 657         instEntry = getInstance(obj, instId);
 658
 659         // If the instance does not exist create it and any other instances before it
 660         if (instEntry == NULL) {
 661             instEntry = createInstance(obj, instId);
 662             if (instEntry == NULL) {
 663                 goto unlock_exit;
 664             }
 665         }
 666         // Set the data
 667         memcpy(InstanceData(instEntry), dataIn, obj->instance_size);
 668     }
 669
 670     // Fire event
 671     sendEvent((struct UAVOBase *)obj_handle, instId, EV_UNPACKED);
 672     rc = 0;
 673
 674 unlock_exit:
 675     xSemaphoreGiveRecursive(mutex);
 676     return rc;
 677 }
 678
 679 /**
 680  * Pack an object to a byte array
 681  * \param[in] obj The object handle
 682  * \param[in] instId The instance ID
 683  * \param[out] dataOut The byte array
 684  * \return 0 if success or -1 if failure
 685  */
 686 int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t *dataOut)
 687 {
 688     PIOS_Assert(obj_handle);
 689
 690     // Lock
 691     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 692
 693     int32_t rc = -1;
 694
 695     if (UAVObjIsMetaobject(obj_handle)) {
 696         if (instId != 0) {
 697             goto unlock_exit;
 698         }
 699         memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes);
 700     } else {
 701         struct UAVOData *obj;
 702         InstanceHandle instEntry;
 703
 704         // Cast handle to object
 705         obj = (struct UAVOData *)obj_handle;
 706
 707         // Get the instance
 708         instEntry = getInstance(obj, instId);
 709         if (instEntry == NULL) {
 710             goto unlock_exit;
 711         }
 712         // Pack data
 713         memcpy(dataOut, InstanceData(instEntry), obj->instance_size);
 714     }
 715
 716     rc = 0;
 717
 718 unlock_exit:
 719     xSemaphoreGiveRecursive(mutex);
 720     return rc;
 721 }
 722
 723 /**
 724  * Update a CRC with an object data
 725  * \param[in] obj The object handle
 726  * \param[in] instId The instance ID
 727  * \param[in] crc The crc to update
 728  * \return the updated crc
 729  */
 730 uint8_t UAVObjUpdateCRC(UAVObjHandle obj_handle, uint16_t instId, uint8_t crc)
 731 {
 732     PIOS_Assert(obj_handle);
 733
 734     // Lock
 735     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 736
 737     if (UAVObjIsMetaobject(obj_handle)) {
 738         if (instId != 0) {
 739             goto unlock_exit;
 740         }
 741         // TODO
 742     } else {
 743         struct UAVOData *obj;
 744         InstanceHandle instEntry;
 745
 746         // Cast handle to object
 747         obj = (struct UAVOData *)obj_handle;
 748
 749         // Get the instance
 750         instEntry = getInstance(obj, instId);
 751         if (instEntry == NULL) {
 752             goto unlock_exit;
 753         }
 754         // Update crc
 755         crc = PIOS_CRC_updateCRC(crc, (uint8_t *)InstanceData(instEntry), (int32_t)obj->instance_size);
 756     }
 757
 758 unlock_exit:
 759     xSemaphoreGiveRecursive(mutex);
 760     return crc;
 761 }
 762
 763 /**
 764  * Actually write the object's data to the logfile
 765  * \param[in] obj The object handle
 766  * \param[in] instId The object instance ID
 767  */
 768 void UAVObjInstanceWriteToLog(UAVObjHandle obj_handle, uint16_t instId)
 769 {
 770     PIOS_Assert(obj_handle);
 771
 772     // Lock
 773     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 774
 775     if (UAVObjIsMetaobject(obj_handle)) {
 776         if (instId != 0) {
 777             goto unlock_exit;
 778         }
 779         PIOS_DEBUGLOG_UAVObject(UAVObjGetID(obj_handle), instId, MetaNumBytes, (uint8_t *)MetaDataPtr((struct UAVOMeta *)obj_handle));
 780     } else {
 781         struct UAVOData *obj;
 782         InstanceHandle instEntry;
 783
 784         // Cast handle to object
 785         obj = (struct UAVOData *)obj_handle;
 786
 787         // Get the instance
 788         instEntry = getInstance(obj, instId);
 789         if (instEntry == NULL) {
 790             goto unlock_exit;
 791         }
 792         // Pack data
 793         PIOS_DEBUGLOG_UAVObject(UAVObjGetID(obj_handle), instId, obj->instance_size, (uint8_t *)InstanceData(instEntry));
 794     }
 795
 796 unlock_exit:
 797     xSemaphoreGiveRecursive(mutex);
 798 }
 799
 800 /**
 801  * Save the data of the specified object to the file system (SD card).
 802  * If the object contains multiple instances, all of them will be saved.
 803  * A new file with the name of the object will be created.
 804  * The object data can be restored using the UAVObjLoad function.
 805  * @param[in] obj The object handle.
 806  * @param[in] instId The instance ID
 807  * @return 0 if success or -1 if failure
 808  */
 809 int32_t UAVObjSave(UAVObjHandle obj_handle, uint16_t instId)
 810 {
 811     PIOS_Assert(obj_handle);
 812
 813     if (UAVObjIsMetaobject(obj_handle)) {
 814         if (instId != 0) {
 815             return -1;
 816         }
 817
 818         if (PIOS_FLASHFS_ObjSave(pios_uavo_settings_fs_id, UAVObjGetID(obj_handle), instId, (uint8_t *)MetaDataPtr((struct UAVOMeta *)obj_handle), UAVObjGetNumBytes(obj_handle)) != 0) {
 819             return -1;
 820         }
 821     } else {
 822         InstanceHandle instEntry = getInstance((struct UAVOData *)obj_handle, instId);
 823
 824         if (instEntry == NULL) {
 825             return -1;
 826         }
 827
 828         if (InstanceData(instEntry) == NULL) {
 829             return -1;
 830         }
 831
 832         if (PIOS_FLASHFS_ObjSave(pios_uavo_settings_fs_id, UAVObjGetID(obj_handle), instId, InstanceData(instEntry), UAVObjGetNumBytes(obj_handle)) != 0) {
 833             return -1;
 834         }
 835     }
 836     return 0;
 837 }
 838
 839
 840 /**
 841  * Load an object from the file system (SD card).
 842  * A file with the name of the object will be opened.
 843  * The object data can be saved using the UAVObjSave function.
 844  * @param[in] obj The object handle.
 845  * @param[in] instId The object instance
 846  * @return 0 if success or -1 if failure
 847  */
 848 int32_t UAVObjLoad(UAVObjHandle obj_handle, uint16_t instId)
 849 {
 850     PIOS_Assert(obj_handle);
 851
 852     if (UAVObjIsMetaobject(obj_handle)) {
 853         if (instId != 0) {
 854             return -1;
 855         }
 856
 857         // Fire event on success
 858         if (PIOS_FLASHFS_ObjLoad(pios_uavo_settings_fs_id, UAVObjGetID(obj_handle), instId, (uint8_t *)MetaDataPtr((struct UAVOMeta *)obj_handle), UAVObjGetNumBytes(obj_handle)) == 0) {
 859             sendEvent((struct UAVOBase *)obj_handle, instId, EV_UNPACKED);
 860         } else {
 861             return -1;
 862         }
 863     } else {
 864         InstanceHandle instEntry = getInstance((struct UAVOData *)obj_handle, instId);
 865
 866         if (instEntry == NULL) {
 867             return -1;
 868         }
 869
 870         // Fire event on success
 871         if (PIOS_FLASHFS_ObjLoad(pios_uavo_settings_fs_id, UAVObjGetID(obj_handle), instId, InstanceData(instEntry), UAVObjGetNumBytes(obj_handle)) == 0) {
 872             sendEvent((struct UAVOBase *)obj_handle, instId, EV_UNPACKED);
 873         } else {
 874             return -1;
 875         }
 876     }
 877
 878
 879     return 0;
 880 }
 881
 882 /**
 883  * Delete an object from the file system (SD card).
 884  * @param[in] obj The object handle.
 885  * @param[in] instId The object instance
 886  * @return 0 if success or -1 if failure
 887  */
 888 int32_t UAVObjDelete(UAVObjHandle obj_handle, uint16_t instId)
 889 {
 890     PIOS_Assert(obj_handle);
 891     PIOS_FLASHFS_ObjDelete(pios_uavo_settings_fs_id, UAVObjGetID(obj_handle), instId);
 892     return 0;
 893 }
 894
 895 /**
 896  * Save all settings objects to the SD card.
 897  * @return 0 if success or -1 if failure
 898  */
 899 int32_t UAVObjSaveSettings()
 900 {
 901     // Get lock
 902     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 903
 904     int32_t rc = -1;
 905
 906     // Save all settings objects
 907     UAVO_LIST_ITERATE(obj)
 908     // Check if this is a settings object
 909     if (UAVObjIsSettings(obj)) {
 910         // Save object
 911         if (UAVObjSave((UAVObjHandle)obj, 0) ==
 912             -1) {
 913             goto unlock_exit;
 914         }
 915     }
 916 }
 917
 918 rc = 0;
 919
 920 unlock_exit:
 921 xSemaphoreGiveRecursive(mutex);
 922 return rc;
 923 }
 924
 925 /**
 926  * Load all settings objects from the SD card.
 927  * @return 0 if success or -1 if failure
 928  */
 929 int32_t UAVObjLoadSettings()
 930 {
 931     // Get lock
 932     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 933
 934     int32_t rc = -1;
 935
 936     // Load all settings objects
 937     UAVO_LIST_ITERATE(obj)
 938     // Check if this is a settings object
 939     if (UAVObjIsSettings(obj)) {
 940         // Load object
 941         if (UAVObjLoad((UAVObjHandle)obj, 0) ==
 942             -1) {
 943             goto unlock_exit;
 944         }
 945     }
 946 }
 947
 948 rc = 0;
 949
 950 unlock_exit:
 951 xSemaphoreGiveRecursive(mutex);
 952 return rc;
 953 }
 954
 955 /**
 956  * Delete all settings objects from the SD card.
 957  * @return 0 if success or -1 if failure
 958  */
 959 int32_t UAVObjDeleteSettings()
 960 {
 961     // Get lock
 962     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 963
 964     int32_t rc = -1;
 965
 966     // Save all settings objects
 967     UAVO_LIST_ITERATE(obj)
 968     // Check if this is a settings object
 969     if (UAVObjIsSettings(obj)) {
 970         // Save object
 971         if (UAVObjDelete((UAVObjHandle)obj, 0)
 972             == -1) {
 973             goto unlock_exit;
 974         }
 975     }
 976 }
 977
 978 rc = 0;
 979
 980 unlock_exit:
 981 xSemaphoreGiveRecursive(mutex);
 982 return rc;
 983 }
 984
 985 /**
 986  * Save all metaobjects to the SD card.
 987  * @return 0 if success or -1 if failure
 988  */
 989 int32_t UAVObjSaveMetaobjects()
 990 {
 991     // Get lock
 992     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 993
 994     int32_t rc = -1;
 995
 996     // Save all settings objects
 997     UAVO_LIST_ITERATE(obj)
 998     // Save object
 999     if (UAVObjSave((UAVObjHandle)MetaObjectPtr(obj), 0) ==
1000         -1) {
1001         goto unlock_exit;
1002     }
1003 }
1004
1005 rc = 0;
1006
1007 unlock_exit:
1008 xSemaphoreGiveRecursive(mutex);
1009 return rc;
1010 }
1011
1012 /**
1013  * Load all metaobjects from the SD card.
1014  * @return 0 if success or -1 if failure
1015  */
1016 int32_t UAVObjLoadMetaobjects()
1017 {
1018     // Get lock
1019     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
1020
1021     int32_t rc = -1;
1022
1023     // Load all settings objects
1024     UAVO_LIST_ITERATE(obj)
1025     // Load object
1026     if (UAVObjLoad((UAVObjHandle)MetaObjectPtr(obj), 0) ==
1027         -1) {
1028         goto unlock_exit;
1029     }
1030 }
1031
1032 rc = 0;
1033
1034 unlock_exit:
1035 xSemaphoreGiveRecursive(mutex);
1036 return rc;
1037 }
1038
1039 /**
1040  * Delete all metaobjects from the SD card.
1041  * @return 0 if success or -1 if failure
1042  */
1043 int32_t UAVObjDeleteMetaobjects()
1044 {
1045     // Get lock
1046     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
1047
1048     int32_t rc = -1;
1049
1050     // Load all settings objects
1051     UAVO_LIST_ITERATE(obj)
1052     // Load object
1053     if (UAVObjDelete((UAVObjHandle)MetaObjectPtr(obj), 0)
1054         == -1) {
1055         goto unlock_exit;
1056     }
1057 }
1058
1059 rc = 0;
1060
1061 unlock_exit:
1062 xSemaphoreGiveRecursive(mutex);
1063 return rc;
1064 }
1065
1066 /**
1067  * Set the object data
1068  * \param[in] obj The object handle
1069  * \param[in] dataIn The object's data structure
1070  * \return 0 if success or -1 if failure
1071  */
1072 int32_t UAVObjSetData(UAVObjHandle obj_handle, const void *dataIn)
1073 {
1074     return UAVObjSetInstanceData(obj_handle, 0, dataIn);
1075 }
1076
1077 /**
1078  * Set the object data
1079  * \param[in] obj The object handle
1080  * \param[in] dataIn The object's data structure
1081  * \return 0 if success or -1 if failure
1082  */
1083 int32_t UAVObjSetDataField(UAVObjHandle obj_handle, const void *dataIn, uint32_t offset, uint32_t size)
1084 {
1085     return UAVObjSetInstanceDataField(obj_handle, 0, dataIn, offset, size);
1086 }
1087
1088 /**
1089  * Get the object data
1090  * \param[in] obj The object handle
1091  * \param[out] dataOut The object's data structure
1092  * \return 0 if success or -1 if failure
1093  */
1094 int32_t UAVObjGetData(UAVObjHandle obj_handle, void *dataOut)
1095 {
1096     return UAVObjGetInstanceData(obj_handle, 0, dataOut);
1097 }
1098
1099 /**
1100  * Get the object data
1101  * \param[in] obj The object handle
1102  * \param[out] dataOut The object's data structure
1103  * \return 0 if success or -1 if failure
1104  */
1105 int32_t UAVObjGetDataField(UAVObjHandle obj_handle, void *dataOut, uint32_t offset, uint32_t size)
1106 {
1107     return UAVObjGetInstanceDataField(obj_handle, 0, dataOut, offset, size);
1108 }
1109
1110 /**
1111  * Set the data of a specific object instance
1112  * \param[in] obj The object handle
1113  * \param[in] instId The object instance ID
1114  * \param[in] dataIn The object's data structure
1115  * \return 0 if success or -1 if failure
1116  */
1117 int32_t UAVObjSetInstanceData(UAVObjHandle obj_handle, uint16_t instId,
1118                               const void *dataIn)
1119 {
1120     PIOS_Assert(obj_handle);
1121
1122     // Lock
1123     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
1124
1125     int32_t rc = -1;
1126
1127     if (UAVObjIsMetaobject(obj_handle)) {
1128         if (instId != 0) {
1129             goto unlock_exit;
1130         }
1131         memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle), dataIn, MetaNumBytes);
1132     } else {
1133         struct UAVOData *obj;
1134         InstanceHandle instEntry;
1135
1136         // Cast to object info
1137         obj = (struct UAVOData *)obj_handle;
1138
1139         // Check access level
1140         if (UAVObjReadOnly(obj_handle)) {
1141             goto unlock_exit;
1142         }
1143         // Get instance information
1144         instEntry = getInstance(obj, instId);
1145         if (instEntry == NULL) {
1146             goto unlock_exit;
1147         }
1148         // Set data
1149         memcpy(InstanceData(instEntry), dataIn, obj->instance_size);
1150     }
1151
1152     // Fire event
1153     sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED);
1154     rc = 0;
1155
1156 unlock_exit:
1157     xSemaphoreGiveRecursive(mutex);
1158     return rc;
1159 }
1160
1161 /**
1162  * Set the data of a specific object instance
1163  * \param[in] obj The object handle
1164  * \param[in] instId The object instance ID
1165  * \param[in] dataIn The object's data structure
1166  * \return 0 if success or -1 if failure
1167  */
1168 int32_t UAVObjSetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, const void *dataIn, uint32_t offset, uint32_t size)
1169 {
1170     PIOS_Assert(obj_handle);
1171
1172     // Lock
1173     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
1174
1175     int32_t rc = -1;
1176
1177     if (UAVObjIsMetaobject(obj_handle)) {
1178         // Get instance information
1179         if (instId != 0) {
1180             goto unlock_exit;
1181         }
1182
1183         // Check for overrun
1184         if ((size + offset) > MetaNumBytes) {
1185             goto unlock_exit;
1186         }
1187
1188         // Set data
1189         memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle) + offset, dataIn, size);
1190     } else {
1191         struct UAVOData *obj;
1192         InstanceHandle instEntry;
1193
1194         // Cast to object info
1195         obj = (struct UAVOData *)obj_handle;
1196
1197         // Check access level
1198         if (UAVObjReadOnly(obj_handle)) {
1199             goto unlock_exit;
1200         }
1201
1202         // Get instance information
1203         instEntry = getInstance(obj, instId);
1204         if (instEntry == NULL) {
1205             goto unlock_exit;
1206         }
1207
1208         // Check for overrun
1209         if ((size + offset) > obj->instance_size) {
1210             goto unlock_exit;
1211         }
1212
1213         // Set data
1214         memcpy(InstanceData(instEntry) + offset, dataIn, size);
1215     }
1216
1217
1218     // Fire event
1219     sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED);
1220     rc = 0;
1221
1222 unlock_exit:
1223     xSemaphoreGiveRecursive(mutex);
1224     return rc;
1225 }
1226
1227 /**
1228  * Get the data of a specific object instance
1229  * \param[in] obj The object handle
1230  * \param[in] instId The object instance ID
1231  * \param[out] dataOut The object's data structure
1232  * \return 0 if success or -1 if failure
1233  */
1234 int32_t UAVObjGetInstanceData(UAVObjHandle obj_handle, uint16_t instId,
1235                               void *dataOut)
1236 {
1237     PIOS_Assert(obj_handle);
1238
1239     // Lock
1240     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
1241
1242     int32_t rc = -1;
1243
1244     if (UAVObjIsMetaobject(obj_handle)) {
1245         // Get instance information
1246         if (instId != 0) {
1247             goto unlock_exit;
1248         }
1249         // Set data
1250         memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes);
1251     } else {
1252         struct UAVOData *obj;
1253         InstanceHandle instEntry;
1254
1255         // Cast to object info
1256         obj = (struct UAVOData *)obj_handle;
1257
1258         // Get instance information
1259         instEntry = getInstance(obj, instId);
1260         if (instEntry == NULL) {
1261             goto unlock_exit;
1262         }
1263         // Set data
1264         memcpy(dataOut, InstanceData(instEntry), obj->instance_size);
1265     }
1266
1267     rc = 0;
1268
1269 unlock_exit:
1270     xSemaphoreGiveRecursive(mutex);
1271     return rc;
1272 }
1273
1274 /**
1275  * Get the data of a specific object instance
1276  * \param[in] obj The object handle
1277  * \param[in] instId The object instance ID
1278  * \param[out] dataOut The object's data structure
1279  * \return 0 if success or -1 if failure
1280  */
1281 int32_t UAVObjGetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, void *dataOut, uint32_t offset, uint32_t size)
1282 {
1283     PIOS_Assert(obj_handle);
1284
1285     // Lock
1286     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
1287
1288     int32_t rc = -1;
1289
1290     if (UAVObjIsMetaobject(obj_handle)) {
1291         // Get instance information
1292         if (instId != 0) {
1293             goto unlock_exit;
1294         }
1295
1296         // Check for overrun
1297         if ((size + offset) > MetaNumBytes) {
1298             goto unlock_exit;
1299         }
1300
1301         // Set data
1302         memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle) + offset, size);
1303     } else {
1304         struct UAVOData *obj;
1305         InstanceHandle instEntry;
1306
1307         // Cast to object info
1308         obj = (struct UAVOData *)obj_handle;
1309
1310         // Get instance information
1311         instEntry = getInstance(obj, instId);
1312         if (instEntry == NULL) {
1313             goto unlock_exit;
1314         }
1315
1316         // Check for overrun
1317         if ((size + offset) > obj->instance_size) {
1318             goto unlock_exit;
1319         }
1320
1321         // Set data
1322         memcpy(dataOut, InstanceData(instEntry) + offset, size);
1323     }
1324
1325     rc = 0;
1326
1327 unlock_exit:
1328     xSemaphoreGiveRecursive(mutex);
1329     return rc;
1330 }
1331
1332 /**
1333  * Set the object metadata
1334  * \param[in] obj The object handle
1335  * \param[in] dataIn The object's metadata structure
1336  * \return 0 if success or -1 if failure
1337  */
1338 int32_t UAVObjSetMetadata(UAVObjHandle obj_handle, const UAVObjMetadata *dataIn)
1339 {
1340     PIOS_Assert(obj_handle);
1341
1342     // Set metadata (metadata of metaobjects can not be modified)
1343     if (UAVObjIsMetaobject(obj_handle)) {
1344         return -1;
1345     }
1346
1347     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
1348
1349     UAVObjSetData((UAVObjHandle)MetaObjectPtr((struct UAVOData *)obj_handle), dataIn);
1350
1351     xSemaphoreGiveRecursive(mutex);
1352     return 0;
1353 }
1354
1355 /**
1356  * Get the object metadata
1357  * \param[in] obj The object handle
1358  * \param[out] dataOut The object's metadata structure
1359  * \return 0 if success or -1 if failure
1360  */
1361 int32_t UAVObjGetMetadata(UAVObjHandle obj_handle, UAVObjMetadata *dataOut)
1362 {
1363     PIOS_Assert(obj_handle);
1364
1365     // Lock
1366     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
1367
1368     // Get metadata
1369     if (UAVObjIsMetaobject(obj_handle)) {
1370         memcpy(dataOut, &defMetadata, sizeof(UAVObjMetadata));
1371     } else {
1372         UAVObjGetData((UAVObjHandle)MetaObjectPtr((struct UAVOData *)obj_handle),
1373                       dataOut);
1374     }
1375
1376     // Unlock
1377     xSemaphoreGiveRecursive(mutex);
1378     return 0;
1379 }
1380
1381 /*******************************
1382  * Object Metadata Manipulation
1383  ******************************/
1384
1385 /**
1386  * Get the UAVObject metadata access member
1387  * \param[in] metadata The metadata object
1388  * \return the access type
1389  */
1390 UAVObjAccessType UAVObjGetAccess(const UAVObjMetadata *metadata)
1391 {
1392     PIOS_Assert(metadata);
1393     return (metadata->flags >> UAVOBJ_ACCESS_SHIFT) & 1;
1394 }
1395
1396 /**
1397  * Set the UAVObject metadata access member
1398  * \param[in] metadata The metadata object
1399  * \param[in] mode The access mode
1400  */
1401 void UAVObjSetAccess(UAVObjMetadata *metadata, UAVObjAccessType mode)
1402 {
1403     PIOS_Assert(metadata);
1404     SET_BITS(metadata->flags, UAVOBJ_ACCESS_SHIFT, mode, 1);
1405 }
1406
1407 /**
1408  * Get the UAVObject metadata GCS access member
1409  * \param[in] metadata The metadata object
1410  * \return the GCS access type
1411  */
1412 UAVObjAccessType UAVObjGetGcsAccess(const UAVObjMetadata *metadata)
1413 {
1414     PIOS_Assert(metadata);
1415     return (metadata->flags >> UAVOBJ_GCS_ACCESS_SHIFT) & 1;
1416 }
1417
1418 /**
1419  * Set the UAVObject metadata GCS access member
1420  * \param[in] metadata The metadata object
1421  * \param[in] mode The access mode
1422  */
1423 void UAVObjSetGcsAccess(UAVObjMetadata *metadata, UAVObjAccessType mode)
1424 {
1425     PIOS_Assert(metadata);
1426     SET_BITS(metadata->flags, UAVOBJ_GCS_ACCESS_SHIFT, mode, 1);
1427 }
1428
1429 /**
1430  * Get the UAVObject metadata telemetry acked member
1431  * \param[in] metadata The metadata object
1432  * \return the telemetry acked boolean
1433  */
1434 uint8_t UAVObjGetTelemetryAcked(const UAVObjMetadata *metadata)
1435 {
1436     PIOS_Assert(metadata);
1437     return (metadata->flags >> UAVOBJ_TELEMETRY_ACKED_SHIFT) & 1;
1438 }
1439
1440 /**
1441  * Set the UAVObject metadata telemetry acked member
1442  * \param[in] metadata The metadata object
1443  * \param[in] val The telemetry acked boolean
1444  */
1445 void UAVObjSetTelemetryAcked(UAVObjMetadata *metadata, uint8_t val)
1446 {
1447     PIOS_Assert(metadata);
1448     SET_BITS(metadata->flags, UAVOBJ_TELEMETRY_ACKED_SHIFT, val, 1);
1449 }
1450
1451 /**
1452  * Get the UAVObject metadata GCS telemetry acked member
1453  * \param[in] metadata The metadata object
1454  * \return the telemetry acked boolean
1455  */
1456 uint8_t UAVObjGetGcsTelemetryAcked(const UAVObjMetadata *metadata)
1457 {
1458     PIOS_Assert(metadata);
1459     return (metadata->flags >> UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT) & 1;
1460 }
1461
1462 /**
1463  * Set the UAVObject metadata GCS telemetry acked member
1464  * \param[in] metadata The metadata object
1465  * \param[in] val The GCS telemetry acked boolean
1466  */
1467 void UAVObjSetGcsTelemetryAcked(UAVObjMetadata *metadata, uint8_t val)
1468 {
1469     PIOS_Assert(metadata);
1470     SET_BITS(metadata->flags, UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT, val, 1);
1471 }
1472
1473 /**
1474  * Get the UAVObject metadata telemetry update mode
1475  * \param[in] metadata The metadata object
1476  * \return the telemetry update mode
1477  */
1478 UAVObjUpdateMode UAVObjGetTelemetryUpdateMode(const UAVObjMetadata *metadata)
1479 {
1480     PIOS_Assert(metadata);
1481     return (metadata->flags >> UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK;
1482 }
1483
1484 /**
1485  * Set the UAVObject metadata telemetry update mode member
1486  * \param[in] metadata The metadata object
1487  * \param[in] val The telemetry update mode
1488  */
1489 void UAVObjSetTelemetryUpdateMode(UAVObjMetadata *metadata, UAVObjUpdateMode val)
1490 {
1491     PIOS_Assert(metadata);
1492     SET_BITS(metadata->flags, UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK);
1493 }
1494
1495 /**
1496  * Get the UAVObject metadata GCS telemetry update mode
1497  * \param[in] metadata The metadata object
1498  * \return the GCS telemetry update mode
1499  */
1500 UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata *metadata)
1501 {
1502     PIOS_Assert(metadata);
1503     return (metadata->flags >> UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK;
1504 }
1505
1506 /**
1507  * Set the UAVObject metadata GCS telemetry update mode member
1508  * \param[in] metadata The metadata object
1509  * \param[in] val The GCS telemetry update mode
1510  */
1511 void UAVObjSetGcsTelemetryUpdateMode(UAVObjMetadata *metadata, UAVObjUpdateMode val)
1512 {
1513     PIOS_Assert(metadata);
1514     SET_BITS(metadata->flags, UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK);
1515 }
1516
1517 /**
1518  * Get the UAVObject metadata logging update mode
1519  * \param[in] metadata The metadata object
1520  * \return the GCS telemetry update mode
1521  */
1522 UAVObjUpdateMode UAVObjGetLoggingUpdateMode(const UAVObjMetadata *metadata)
1523 {
1524     PIOS_Assert(metadata);
1525     return (metadata->flags >> UAVOBJ_LOGGING_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK;
1526 }
1527
1528 /**
1529  * Set the UAVObject metadata logging update mode member
1530  * \param[in] metadata The metadata object
1531  * \param[in] val The GCS telemetry update mode
1532  */
1533 void UAVObjSetLoggingUpdateMode(UAVObjMetadata *metadata, UAVObjUpdateMode val)
1534 {
1535     PIOS_Assert(metadata);
1536     SET_BITS(metadata->flags, UAVOBJ_LOGGING_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK);
1537 }
1538
1539
1540 /**
1541  * Check if an object is read only
1542  * \param[in] obj The object handle
1543  * \return
1544  *   \arg 0 if not read only
1545  *   \arg 1 if read only
1546  *   \arg -1 if unable to get meta data
1547  */
1548 int8_t UAVObjReadOnly(UAVObjHandle obj_handle)
1549 {
1550     PIOS_Assert(obj_handle);
1551     if (!UAVObjIsMetaobject(obj_handle)) {
1552         return UAVObjGetAccess(LinkedMetaDataPtr((struct UAVOData *)obj_handle)) == ACCESS_READONLY;
1553     }
1554     return -1;
1555 }
1556
1557 /**
1558  * Connect an event queue to the object, if the queue is already connected then the event mask is only updated.
1559  * All events matching the event mask will be pushed to the event queue.
1560  * \param[in] obj The object handle
1561  * \param[in] queue The event queue
1562  * \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
1563  * \return 0 if success or -1 if failure
1564  */
1565 int32_t UAVObjConnectQueue(UAVObjHandle obj_handle, xQueueHandle queue,
1566                            uint8_t eventMask)
1567 {
1568     PIOS_Assert(obj_handle);
1569     PIOS_Assert(queue);
1570     int32_t res;
1571     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
1572     res = connectObj(obj_handle, queue, 0, eventMask);
1573     xSemaphoreGiveRecursive(mutex);
1574     return res;
1575 }
1576
1577 /**
1578  * Disconnect an event queue from the object.
1579  * \param[in] obj The object handle
1580  * \param[in] queue The event queue
1581  * \return 0 if success or -1 if failure
1582  */
1583 int32_t UAVObjDisconnectQueue(UAVObjHandle obj_handle, xQueueHandle queue)
1584 {
1585     PIOS_Assert(obj_handle);
1586     PIOS_Assert(queue);
1587     int32_t res;
1588     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
1589     res = disconnectObj(obj_handle, queue, 0);
1590     xSemaphoreGiveRecursive(mutex);
1591     return res;
1592 }
1593
1594 /**
1595  * Connect an event callback to the object, if the callback is already connected then the event mask is only updated.
1596  * The supplied callback will be invoked on all events matching the event mask.
1597  * \param[in] obj The object handle
1598  * \param[in] cb The event callback
1599  * \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
1600  * \return 0 if success or -1 if failure
1601  */
1602 int32_t UAVObjConnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb,
1603                               uint8_t eventMask)
1604 {
1605     PIOS_Assert(obj_handle);
1606     int32_t res;
1607     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
1608     res = connectObj(obj_handle, 0, cb, eventMask);
1609     xSemaphoreGiveRecursive(mutex);
1610     return res;
1611 }
1612
1613 /**
1614  * Disconnect an event callback from the object.
1615  * \param[in] obj The object handle
1616  * \param[in] cb The event callback
1617  * \return 0 if success or -1 if failure
1618  */
1619 int32_t UAVObjDisconnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb)
1620 {
1621     PIOS_Assert(obj_handle);
1622     int32_t res;
1623     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
1624     res = disconnectObj(obj_handle, 0, cb);
1625     xSemaphoreGiveRecursive(mutex);
1626     return res;
1627 }
1628
1629 /**
1630  * Request an update of the object's data from the GCS. The call will not wait for the response, a EV_UPDATED event
1631  * will be generated as soon as the object is updated.
1632  * \param[in] obj The object handle
1633  */
1634 void UAVObjRequestUpdate(UAVObjHandle obj_handle)
1635 {
1636     UAVObjRequestInstanceUpdate(obj_handle, UAVOBJ_ALL_INSTANCES);
1637 }
1638
1639 /**
1640  * Request an update of the object's data from the GCS.
1641  * The call will not wait for the response, a EV_UPDATED event will be generated as soon as the object is updated.
1642  * \param[in] obj The object handle
1643  * \param[in] instId Object instance ID to update
1644  */
1645 void UAVObjRequestInstanceUpdate(UAVObjHandle obj_handle, uint16_t instId)
1646 {
1647     PIOS_Assert(obj_handle);
1648     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
1649     sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATE_REQ);
1650     xSemaphoreGiveRecursive(mutex);
1651 }
1652
1653 /**
1654  * Trigger a EV_UPDATED_MANUAL event for an object.
1655  * \param[in] obj The object handle
1656  */
1657 void UAVObjUpdated(UAVObjHandle obj_handle)
1658 {
1659     UAVObjInstanceUpdated(obj_handle, UAVOBJ_ALL_INSTANCES);
1660 }
1661
1662 /**
1663  * Trigger a EV_UPDATED_MANUAL event for an object instance.
1664  * \param[in] obj The object handle
1665  * \param[in] instId The object instance ID
1666  */
1667 void UAVObjInstanceUpdated(UAVObjHandle obj_handle, uint16_t instId)
1668 {
1669     PIOS_Assert(obj_handle);
1670     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
1671     sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED_MANUAL);
1672     xSemaphoreGiveRecursive(mutex);
1673 }
1674
1675 /**
1676  * Trigger a EV_UPDATED event for an object instance.
1677  * \param[in] obj The object handle
1678  * \param[in] instId The object instance ID
1679  */
1680 static void instanceAutoUpdated(UAVObjHandle obj_handle, uint16_t instId)
1681 {
1682     PIOS_Assert(obj_handle);
1683     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
1684     sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED);
1685     xSemaphoreGiveRecursive(mutex);
1686 }
1687
1688 /*
1689  * Log the object's data (triggers a EV_LOGGING_MANUAL event on this object).
1690  * \param[in] obj The object handle
1691  */
1692 void UAVObjLogging(UAVObjHandle obj_handle)
1693 {
1694     UAVObjInstanceLogging(obj_handle, UAVOBJ_ALL_INSTANCES);
1695 }
1696
1697 /**
1698  * Log the object's data (triggers a EV_LOGGING_MANUAL event on this object).
1699  * \param[in] obj The object handle
1700  * \param[in] instId The object instance ID
1701  */
1702 void UAVObjInstanceLogging(UAVObjHandle obj_handle, uint16_t instId)
1703 {
1704     PIOS_Assert(obj_handle);
1705     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
1706     sendEvent((struct UAVOBase *)obj_handle, instId, EV_LOGGING_MANUAL);
1707     xSemaphoreGiveRecursive(mutex);
1708 }
1709
1710 /**
1711  * Iterate through all objects in the list.
1712  * \param iterator This function will be called once for each object,
1713  * the object will be passed as a parameter
1714  */
1715 void UAVObjIterate(void (*iterator)(UAVObjHandle obj))
1716 {
1717     PIOS_Assert(iterator);
1718
1719     // Get lock
1720     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
1721
1722     // Iterate through the list and invoke iterator for each object
1723     UAVO_LIST_ITERATE (obj)
1724     (*iterator)((UAVObjHandle)obj);
1725     (*iterator)((UAVObjHandle) &obj->metaObj);
1726 }
1727
1728 // Release lock
1729 xSemaphoreGiveRecursive(mutex);
1730 }
1731
1732 /**
1733  * Send a triggered event to all event queues registered on the object.
1734  */
1735 static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId, UAVObjEventType triggered_event)
1736 {
1737     /* Set up the message that will be sent to all registered listeners */
1738     UAVObjEvent msg = {
1739         .obj    = (UAVObjHandle)obj,
1740         .event  = triggered_event,
1741         .instId = instId,
1742         .lowPriority = false,
1743     };
1744
1745     // Go through each object and push the event message in the queue (if event is activated for the queue)
1746     struct ObjectEventEntry *event;
1747
1748     LL_FOREACH(obj->next_event, event) {
1749         if (event->eventMask == 0 || (event->eventMask & triggered_event) != 0) {
1750             // Send to queue if a valid queue is registered
1751             if (event->queue) {
1752                 // will not block
1753                 if (xQueueSend(event->queue, &msg, 0) != pdTRUE) {
1754                     ++stats.eventQueueErrors;
1755                     stats.lastQueueErrorID = UAVObjGetID(obj);
1756                 }
1757             }
1758
1759             // Invoke callback (from event task) if a valid one is registered
1760             if (event->cb) {
1761                 // invoke callback from the event task, will not block
1762                 if (EventCallbackDispatch(&msg, event->cb) != pdTRUE) {
1763                     ++stats.eventCallbackErrors;
1764                     stats.lastCallbackErrorID = UAVObjGetID(obj);
1765                 }
1766             }
1767         }
1768     }
1769
1770     return 0;
1771 }
1772
1773 /**
1774  * Create a new object instance, return the instance info or NULL if failure.
1775  */
1776 static InstanceHandle createInstance(struct UAVOData *obj, uint16_t instId)
1777 {
1778     struct UAVOMultiInst *instEntry;
1779
1780     /* Don't allow more than one instance for single instance objects */
1781     if (UAVObjIsSingleInstance(&(obj->base))) {
1782         PIOS_Assert(0);
1783         return NULL;
1784     }
1785
1786     /* Don't create more than the allowed number of instances */
1787     if (instId >= UAVOBJ_MAX_INSTANCES) {
1788         return NULL;
1789     }
1790
1791     /* Don't allow duplicate instances */
1792     if (instId < UAVObjGetNumInstances(&(obj->base))) {
1793         return NULL;
1794     }
1795
1796     // Create any missing instances (all instance IDs must be sequential)
1797     for (uint16_t n = UAVObjGetNumInstances(&(obj->base)); n < instId; ++n) {
1798         if (createInstance(obj, n) == NULL) {
1799             return NULL;
1800         }
1801     }
1802
1803     /* Create the actual instance */
1804     uint32_t size = sizeof(struct UAVOMultiInst) + obj->instance_size;
1805     instEntry = (struct UAVOMultiInst *)pios_malloc(size);
1806     if (!instEntry) {
1807         return NULL;
1808     }
1809     memset(instEntry, 0, size);
1810     LL_APPEND(((struct UAVOMulti *)obj)->instance0.next, instEntry);
1811
1812     ((struct UAVOMulti *)obj)->num_instances++;
1813
1814     // Fire event
1815     instanceAutoUpdated((UAVObjHandle)obj, instId);
1816
1817     // Done
1818     return InstanceDataOffset(instEntry);
1819 }
1820
1821 /**
1822  * Get the instance information or NULL if the instance does not exist
1823  */
1824 static InstanceHandle getInstance(struct UAVOData *obj, uint16_t instId)
1825 {
1826     if (UAVObjIsMetaobject(&obj->base)) {
1827         /* Metadata Instance */
1828
1829         if (instId != 0) {
1830             return NULL;
1831         }
1832
1833         /* Augment our pointer to reflect the proper type */
1834         struct UAVOMeta *uavo_meta = (struct UAVOMeta *)obj;
1835         return &(uavo_meta->instance0);
1836     } else if (UAVObjIsSingleInstance(&(obj->base))) {
1837         /* Single Instance */
1838
1839         if (instId != 0) {
1840             return NULL;
1841         }
1842
1843         /* Augment our pointer to reflect the proper type */
1844         struct UAVOSingle *uavo_single = (struct UAVOSingle *)obj;
1845         return &(uavo_single->instance0);
1846     } else {
1847         /* Multi Instance */
1848         /* Augment our pointer to reflect the proper type */
1849         struct UAVOMulti *uavo_multi = (struct UAVOMulti *)obj;
1850         if (instId >= uavo_multi->num_instances) {
1851             return NULL;
1852         }
1853
1854         // Look for specified instance ID
1855         uint16_t instance = 0;
1856         struct UAVOMultiInst *instEntry;
1857         LL_FOREACH(&(uavo_multi->instance0), instEntry) {
1858             if (instance++ == instId) {
1859                 /* Found it */
1860                 return &(instEntry->instance);
1861             }
1862         }
1863         /* Instance was not found */
1864         return NULL;
1865     }
1866 }
1867
1868 /**
1869  * Connect an event queue to the object, if the queue is already connected then the event mask is only updated.
1870  * \param[in] obj The object handle
1871  * \param[in] queue The event queue
1872  * \param[in] cb The event callback
1873  * \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
1874  * \return 0 if success or -1 if failure
1875  */
1876 static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue,
1877                           UAVObjEventCallback cb, uint8_t eventMask)
1878 {
1879     struct ObjectEventEntry *event;
1880     struct UAVOBase *obj;
1881
1882     // Check that the queue is not already connected, if it is simply update event mask
1883     obj = (struct UAVOBase *)obj_handle;
1884     LL_FOREACH(obj->next_event, event) {
1885         if (event->queue == queue && event->cb == cb) {
1886             // Already connected, update event mask and return
1887             event->eventMask = eventMask;
1888             return 0;
1889         }
1890     }
1891
1892     // Add queue to list
1893     event = (struct ObjectEventEntry *)pios_malloc(sizeof(struct ObjectEventEntry));
1894     if (event == NULL) {
1895         return -1;
1896     }
1897     event->queue     = queue;
1898     event->cb        = cb;
1899     event->eventMask = eventMask;
1900     LL_APPEND(obj->next_event, event);
1901
1902     // Done
1903     return 0;
1904 }
1905
1906 /**
1907  * Disconnect an event queue from the object
1908  * \param[in] obj The object handle
1909  * \param[in] queue The event queue
1910  * \param[in] cb The event callback
1911  * \return 0 if success or -1 if failure
1912  */
1913 static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue,
1914                              UAVObjEventCallback cb)
1915 {
1916     struct ObjectEventEntry *event;
1917     struct UAVOBase *obj;
1918
1919     // Find queue and remove it
1920     obj = (struct UAVOBase *)obj_handle;
1921     LL_FOREACH(obj->next_event, event) {
1922         if ((event->queue == queue
1923              && event->cb == cb)) {
1924             LL_DELETE(obj->next_event, event);
1925             vPortFree(event);
1926             return 0;
1927         }
1928     }
1929
1930     // If this point is reached the queue was not found
1931     return -1;
1932 }