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