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