diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index f499d549a..7c3895534 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -205,6 +205,7 @@ SRC += $(PIOSSTM32F10X)/pios_usb_hid_prop.c SRC += $(PIOSSTM32F10X)/pios_usb_hid_pwr.c ## PIOS Hardware (Common) +SRC += $(PIOSCOMMON)/pios_flashfs_objlist.c SRC += $(PIOSCOMMON)/pios_flash_w25x.c SRC += $(PIOSCOMMON)/pios_adxl345.c SRC += $(PIOSCOMMON)/pios_com.c diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index 45164052f..c5943ca3c 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -678,8 +678,10 @@ void PIOS_Board_Init(void) { PIOS_DEBUG_Assert(0); } - PIOS_Flash_W25X_Init(pios_spi_flash_accel_id); + PIOS_Flash_W25X_Init(pios_spi_flash_accel_id); PIOS_ADXL345_Attach(pios_spi_flash_accel_id); + + PIOS_FLASHFS_Init(); #if defined(PIOS_INCLUDE_SPEKTRUM) /* SPEKTRUM init must come before comms */ diff --git a/flight/PiOS/Common/pios_flash_w25x.c b/flight/PiOS/Common/pios_flash_w25x.c index b1c602422..624f76ef8 100644 --- a/flight/PiOS/Common/pios_flash_w25x.c +++ b/flight/PiOS/Common/pios_flash_w25x.c @@ -222,6 +222,14 @@ int8_t PIOS_Flash_W25X_WriteData(uint32_t addr, uint8_t * data, uint16_t len) return 0; } +/** + * @brief Read data from a location in flash memory + * @param[in] addr Address in flash to write to + * @param[in] data Pointer to data to write from flash + * @param[in] len Length of data to write (max 256 bytes) + * @return Zero if success or error code + * @retval -1 Unable to claim SPI bus + */ int8_t PIOS_Flash_W25X_ReadData(uint32_t addr, uint8_t * data, uint16_t len) { if(PIOS_Flash_W25X_ClaimBus() == -1) diff --git a/flight/PiOS/Common/pios_flashfs_objlist.c b/flight/PiOS/Common/pios_flashfs_objlist.c new file mode 100644 index 000000000..5c8248f54 --- /dev/null +++ b/flight/PiOS/Common/pios_flashfs_objlist.c @@ -0,0 +1,294 @@ +/** + ****************************************************************************** + * + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_FLASHFS_OBJLIST Object list based flash filesystem (low ram) + * @{ + * + * @file pios_flashfs_objlist.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief A file system for storing UAVObject in flash chip + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + + +#include "openpilot.h" +#include "uavobjectmanager.h" + +// Private functions +static int32_t PIOS_FLASHFS_CleabObjectTableHeader(); +static int32_t PIOS_FLASHFS_GetObjAddress(uint32_t objId, uint16_t instId); +static int32_t PIOS_FLASHFS_GetNewAddress(uint32_t objId, uint16_t instId); + +// Private variables +static int32_t numObjects = -1; + +// Private structures +// Header for objects in the file system table +struct objectHeader { + uint32_t objMagic; + uint32_t objId; + uint32_t instId; + uint32_t address; +} __attribute__((packed));; + +struct fileHeader { + uint32_t id; + uint16_t instId; + uint16_t size; +} __attribute__((packed)); + + +#define OBJECT_TABLE_MAGIC 0x85FB3C33 +#define OBJ_MAGIC 0x3015AE71 +#define OBJECT_TABLE_START 0x00000010 +#define OBJECT_TABLE_END 0x00001000 +#define SECTOR_SIZE 0x00001000 + +/** + * @brief Initialize the flash object setting FS + * @return 0 if success, -1 if failure + */ +int32_t PIOS_FLASHFS_Init() +{ + + // Check for valid object table or create one + uint32_t object_table_magic; + if (PIOS_Flash_W25X_ReadData(0, (uint8_t *)&object_table_magic, sizeof(object_table_magic)) != 0) + return -1; + if(object_table_magic != OBJECT_TABLE_MAGIC) { + if(PIOS_FLASHFS_CleabObjectTableHeader() < 0) + return -1; + } + + int32_t addr = OBJECT_TABLE_START; + struct objectHeader header; + numObjects = 0; + + // Loop through header area while objects detect to count how many saved + while(addr < OBJECT_TABLE_END) { + // Read the instance data + if (PIOS_Flash_W25X_ReadData(addr, (uint8_t *)&header, sizeof(header)) != 0) + return -1; + + // Counting number of valid headers + if(header.objMagic != OBJ_MAGIC) + break; + + numObjects++; + addr += sizeof(header); + } + + return 0; +} + +/** + * @brief Erase the headers for all objects in the flash chip + * @return 0 if successful, -1 if not + */ +static int32_t PIOS_FLASHFS_CleabObjectTableHeader() +{ + if(PIOS_Flash_W25X_EraseSector(OBJECT_TABLE_START) != 0) + return -1; + + uint32_t object_table_magic = OBJECT_TABLE_MAGIC; + if (PIOS_Flash_W25X_WriteData(0, (uint8_t *)&object_table_magic, sizeof(object_table_magic)) != 0) + return -1; + + return 0; +} + +/** + * @brief Get the address of an object + * @param obj UAVObjHandle for that object + * @parma instId Instance id for that object + * @return address if successful, -1 if not found + */ +static int32_t PIOS_FLASHFS_GetObjAddress(uint32_t objId, uint16_t instId) +{ + int32_t addr = OBJECT_TABLE_START; + struct objectHeader header; + + // Loop through header area while objects detect to count how many saved + while(addr < OBJECT_TABLE_END) { + // Read the instance data + if (PIOS_Flash_W25X_ReadData(addr, (uint8_t *) &header, sizeof(header)) != 0) + return -1; + if(header.objMagic != OBJ_MAGIC) + break; // stop searching once hit first non-object header + else if (header.objId == objId && header.instId == instId) + break; + addr += sizeof(header); + } + + if (header.objId == objId && header.instId == instId) + return header.address; + + return -1; +} + +/** + * @brief Returns an address for a new object and creates entry into object table + * @param[in] obj Object handle for object to be saved + * @param[in] instId The instance id of object to be saved + * @return 0 if success or error code + * @retval -1 Object not found + * @retval -2 No room in object table + * @retval -3 Unable to write entry into object table + * @retval -4 FS not initialized + */ +int32_t PIOS_FLASHFS_GetNewAddress(uint32_t objId, uint16_t instId) +{ + struct objectHeader header; + + if(numObjects < 0) + return -4; + + // Don't worry about max size of flash chip here, other code will catch that + header.objMagic = OBJ_MAGIC; + header.objId = objId; + header.instId = instId; + header.address = OBJECT_TABLE_END + SECTOR_SIZE * numObjects; + + int32_t addr = OBJECT_TABLE_START + sizeof(header) * numObjects; + + // No room for this header in object table + if((addr + sizeof(header)) > OBJECT_TABLE_END) + return -2; + + if(PIOS_Flash_W25X_WriteData(addr, (uint8_t *) &header, sizeof(header)) != 0) + return -3; + + // This numObejcts value must stay consistent or there will be a break in the table + // and later the table will have bad values in it + numObjects++; + return header.address; +} + + +/** + * @brief Saves one object instance per sector + * @param[in] obj UAVObjHandle the object to save + * @param[in] instId The instance of the object to save + * @return 0 if success or -1 if failure + * @note This uses one sector on the flash chip per object so that no buffering in ram + * must be done when erasing the sector before a save + */ +int32_t PIOS_FLASHFS_ObjSave(UAVObjHandle obj, uint16_t instId, uint8_t * data) +{ + uint32_t objId = UAVObjGetID(obj); + + int32_t addr = PIOS_FLASHFS_GetObjAddress(objId, instId); + + // Object currently not saved + if(addr < 0) + addr = PIOS_FLASHFS_GetNewAddress(objId, instId); + + // Could not allocate a sector + if(addr < 0) + return -1; + + struct fileHeader header = { + .id = objId, + .instId = instId, + .size = UAVObjGetNumBytes(obj) + }; + + if(PIOS_Flash_W25X_EraseSector(addr) != 0) + return -2; + + // Save header + // This information IS redundant with the object table id. Oh well. Better safe than sorry. + + if(PIOS_Flash_W25X_WriteData(addr, (uint8_t *) &header, sizeof(header)) != 0) + return -3; + + // Save data + if(PIOS_Flash_W25X_WriteData(addr + sizeof(header), data, UAVObjGetNumBytes(obj)) != 0) + return -4; + + return 0; +} + +/** + * @brief Load one object instance per sector + * @param[in] obj UAVObjHandle the object to save + * @param[in] instId The instance of the object to save + * @return 0 if success or error code + * @retval -1 if object not in file table + * @retval -2 if unable to retrieve object header + * @retval -3 if loaded data instId or objId don't match + * @retval -4 if unable to retrieve instance data + * @note This uses one sector on the flash chip per object so that no buffering in ram + * must be done when erasing the sector before a save + */ +int32_t PIOS_FLASHFS_ObjLoad(UAVObjHandle obj, uint16_t instId, uint8_t * data) +{ + uint32_t objId = UAVObjGetID(obj); + + int32_t addr = PIOS_FLASHFS_GetObjAddress(objId, instId); + + // Object currently not saved + if(addr < 0) + return -1; + + struct fileHeader header; + + // Load header + // This information IS redundant with the object table id. Oh well. Better safe than sorry. + if(PIOS_Flash_W25X_ReadData(addr, (uint8_t *) &header, sizeof(header)) != 0) + return -2; + + if((header.id != objId) || (header.instId != instId)) + return -3; + + // Read the instance data + if (PIOS_Flash_W25X_ReadData(addr + sizeof(header), data, UAVObjGetNumBytes(obj)) != 0) + return -4; + + return 0; +} + +/** + * @brief Delete object from flash + * @param[in] obj UAVObjHandle the object to save + * @param[in] instId The instance of the object to save + * @return 0 if success or error code + * @retval -1 if object not in file table + * @retval -2 Erase failed + * @note To avoid buffering the file table (1k ram!) the entry in the file table + * remains but destination sector is erased. This will make the load fail as the + * file header won't match the object. At next save it goes back there. + */ +int32_t PIOS_FLASHFS_ObjDelete(UAVObjHandle obj, uint16_t instId) +{ + uint32_t objId = UAVObjGetID(obj); + + int32_t addr = PIOS_FLASHFS_GetObjAddress(objId, instId); + + // Object currently not saved + if(addr < 0) + return -1; + + if(PIOS_Flash_W25X_EraseSector(addr) != 0) + return -2; + + return 0; +} \ No newline at end of file diff --git a/flight/PiOS/inc/pios_flashfs_objlist.h b/flight/PiOS/inc/pios_flashfs_objlist.h new file mode 100644 index 000000000..945df4068 --- /dev/null +++ b/flight/PiOS/inc/pios_flashfs_objlist.h @@ -0,0 +1,37 @@ +/** + ****************************************************************************** + * + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_FLASHFS_OBJLIST Object list based flash filesystem (low ram) + * @{ + * + * @file pios_flashfs_objlist.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief A file system for storing UAVObject in flash chip + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "openpilot.h" +#include "uavobjectmanager.h" + +int32_t PIOS_FLASHFS_Init(); +int32_t PIOS_FLASHFS_ObjSave(UAVObjHandle obj, uint16_t instId, uint8_t * data); +int32_t PIOS_FLASHFS_ObjLoad(UAVObjHandle obj, uint16_t instId, uint8_t * data); +int32_t PIOS_FLASHFS_ObjDelete(UAVObjHandle obj, uint16_t instId); \ No newline at end of file diff --git a/flight/PiOS/pios.h b/flight/PiOS/pios.h index cbf2a2249..0fdb03570 100644 --- a/flight/PiOS/pios.h +++ b/flight/PiOS/pios.h @@ -1,135 +1,136 @@ -/** - ****************************************************************************** - * - * @file pios.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Main PiOS header. - * - Central header for the project. - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - - -#ifndef PIOS_H -#define PIOS_H - -/* PIOS Feature Selection */ -#include "pios_config.h" - -#if defined(PIOS_INCLUDE_FREERTOS) -/* FreeRTOS Includes */ -#include "FreeRTOS.h" -#include "task.h" -#include "queue.h" -#include "semphr.h" -#endif - -/* C Lib Includes */ -#include -#include -#include -#include -#include - -/* STM32 Std Perf Lib */ -#include -#include - -#if defined(PIOS_INCLUDE_SDCARD) -/* Dosfs Includes */ -#include - -/* Mass Storage Device Includes */ -#include -#endif - -/* Generic initcall infrastructure */ -#include "pios_initcall.h" - -/* PIOS Board Specific Device Configuration */ -#include "pios_board.h" - -/* PIOS Hardware Includes (STM32F10x) */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#if defined(PIOS_INCLUDE_EXTI) -#include -#endif -#include - -/* PIOS Hardware Includes (Common) */ -#include -#include -#if defined(PIOS_INCLUDE_BMP085) -#include -#endif -#if defined(PIOS_INCLUDE_HCSR04) -#include -#endif -#if defined(PIOS_INCLUDE_HMC5843) -#include -#endif -#if defined(PIOS_INCLUDE_HMC5883) -#include -#endif -#if defined(PIOS_INCLUDE_I2C_ESC) -#include -#endif -#if defined(PIOS_INCLUDE_IMU3000) -#include -#endif -#include - -#if defined(PIOS_INCLUDE_ADXL345) -#include -#endif - -#if defined(PIOS_INCLUDE_BMA180) -#include -#endif - -#if defined(PIOS_INCLUDE_FLASH) -#include -#endif - -#if defined(PIOS_INCLUDE_BL_HELPER) -#include -#endif - -#if defined(PIOS_INCLUDE_USB) -/* USB Libs */ -#include -#endif - -#define NELEMENTS(x) (sizeof(x) / sizeof(*(x))) - -#endif /* PIOS_H */ +/** + ****************************************************************************** + * + * @file pios.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Main PiOS header. + * - Central header for the project. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + + +#ifndef PIOS_H +#define PIOS_H + +/* PIOS Feature Selection */ +#include "pios_config.h" + +#if defined(PIOS_INCLUDE_FREERTOS) +/* FreeRTOS Includes */ +#include "FreeRTOS.h" +#include "task.h" +#include "queue.h" +#include "semphr.h" +#endif + +/* C Lib Includes */ +#include +#include +#include +#include +#include + +/* STM32 Std Perf Lib */ +#include +#include + +#if defined(PIOS_INCLUDE_SDCARD) +/* Dosfs Includes */ +#include + +/* Mass Storage Device Includes */ +#include +#endif + +/* Generic initcall infrastructure */ +#include "pios_initcall.h" + +/* PIOS Board Specific Device Configuration */ +#include "pios_board.h" + +/* PIOS Hardware Includes (STM32F10x) */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#if defined(PIOS_INCLUDE_EXTI) +#include +#endif +#include + +/* PIOS Hardware Includes (Common) */ +#include +#include +#if defined(PIOS_INCLUDE_BMP085) +#include +#endif +#if defined(PIOS_INCLUDE_HCSR04) +#include +#endif +#if defined(PIOS_INCLUDE_HMC5843) +#include +#endif +#if defined(PIOS_INCLUDE_HMC5883) +#include +#endif +#if defined(PIOS_INCLUDE_I2C_ESC) +#include +#endif +#if defined(PIOS_INCLUDE_IMU3000) +#include +#endif +#include + +#if defined(PIOS_INCLUDE_ADXL345) +#include +#endif + +#if defined(PIOS_INCLUDE_BMA180) +#include +#endif + +#if defined(PIOS_INCLUDE_FLASH) +#include +#include +#endif + +#if defined(PIOS_INCLUDE_BL_HELPER) +#include +#endif + +#if defined(PIOS_INCLUDE_USB) +/* USB Libs */ +#include +#endif + +#define NELEMENTS(x) (sizeof(x) / sizeof(*(x))) + +#endif /* PIOS_H */ diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 5dd95349b..7095dfad5 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -3171,6 +3171,8 @@ 65FF4BE913791C3300146BE4 /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; path = Makefile; sourceTree = ""; }; 65FF4BEA13791C3300146BE4 /* op_dfu.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = op_dfu.c; sourceTree = ""; }; 65FF4BEB13791C3300146BE4 /* pios_board.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_board.c; sourceTree = ""; }; + 65FF4D5E137EDEC100146BE4 /* pios_flashfs_objlist.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_flashfs_objlist.c; sourceTree = ""; }; + 65FF4D61137EFA4F00146BE4 /* pios_flashfs_objlist.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_flashfs_objlist.h; sourceTree = ""; }; /* End PBXFileReference section */ /* Begin PBXGroup section */ @@ -7657,6 +7659,7 @@ 65E8F03711EFF25C00BBF654 /* printf-stdarg.c */, 6528CCB412E406B800CF5144 /* pios_adxl345.c */, 6512D60712ED4CB8008175E5 /* pios_flash_w25x.c */, + 65FF4D5E137EDEC100146BE4 /* pios_flashfs_objlist.c */, ); name = Common; path = ../../PiOS/Common; @@ -7681,6 +7684,7 @@ 65E8F03E11EFF25C00BBF654 /* pios_debug.h */, 65E8F03F11EFF25C00BBF654 /* pios_delay.h */, 65E8F04011EFF25C00BBF654 /* pios_exti.h */, + 65FF4D61137EFA4F00146BE4 /* pios_flashfs_objlist.h */, 65E8F04111EFF25C00BBF654 /* pios_gpio.h */, 65E8F04211EFF25C00BBF654 /* pios_hmc5843.h */, 65E8F04311EFF25C00BBF654 /* pios_i2c.h */, diff --git a/flight/UAVObjects/uavobjectmanager.c b/flight/UAVObjects/uavobjectmanager.c index 50558d411..ef1774628 100644 --- a/flight/UAVObjects/uavobjectmanager.c +++ b/flight/UAVObjects/uavobjectmanager.c @@ -1,1607 +1,1498 @@ -/** - ****************************************************************************** - * @addtogroup UAVObjects OpenPilot UAVObjects - * @{ - * @addtogroup UAV Object Manager - * @brief The core UAV Objects functions, most of which are wrappered by - * autogenerated defines - * @{ - * - * - * @file uavobjectmanager.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Object manager library. This library holds a collection of all objects. - * It can be used by all modules/libraries to find an object reference. - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "openpilot.h" - -// Constants - -// Private types - -/** - * List of event queues and the eventmask associated with the queue. - */ -struct ObjectEventListStruct { - xQueueHandle queue; - UAVObjEventCallback cb; - int32_t eventMask; - struct ObjectEventListStruct* next; -}; -typedef struct ObjectEventListStruct ObjectEventList; - -/** - * List of object instances, holds the actual data structure and instance ID - */ -struct ObjectInstListStruct { - void* data; - uint16_t instId; - struct ObjectInstListStruct* next; -}; -typedef struct ObjectInstListStruct ObjectInstList; - -/** - * List of objects registered in the object manager - */ -struct ObjectListStruct { - uint32_t id; /** The object ID */ - const char* name; /** The object name */ - int8_t isMetaobject; /** Set to 1 if this is a metaobject */ - int8_t isSingleInstance; /** Set to 1 if this object has a single instance */ - int8_t isSettings; /** Set to 1 if this object is a settings object */ - uint16_t numBytes; /** Number of data bytes contained in the object (for a single instance) */ - uint16_t numInstances; /** Number of instances */ - struct ObjectListStruct* linkedObj; /** Linked object, for regular objects this is the metaobject and for metaobjects it is the parent object */ - ObjectInstList instances; /** List of object instances, instance 0 always exists */ - ObjectEventList* events; /** Event queues registered on the object */ - struct ObjectListStruct* next; /** Needed by linked list library (utlist.h) */ -}; -typedef struct ObjectListStruct ObjectList; - -// Private functions -static int32_t sendEvent(ObjectList* obj, uint16_t instId, UAVObjEventType event); -static ObjectInstList* createInstance(ObjectList* obj, uint16_t instId); -static ObjectInstList* getInstance(ObjectList* obj, uint16_t instId); -static int32_t connectObj(UAVObjHandle obj, xQueueHandle queue, UAVObjEventCallback cb, int32_t eventMask); -static int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue, UAVObjEventCallback cb); - -#if defined(PIOS_INCLUDE_SDCARD) -static void objectFilename(ObjectList* obj, uint8_t* filename); -static void customSPrintf(uint8_t* buffer, uint8_t* format, ...); -#endif - -// Private variables -static ObjectList* objList; -static xSemaphoreHandle mutex; -static UAVObjMetadata defMetadata; -static UAVObjStats stats; - -/** - * Initialize the object manager - * \return 0 Success - * \return -1 Failure - */ -int32_t UAVObjInitialize() -{ - // Initialize variables - objList = NULL; - memset(&stats, 0, sizeof(UAVObjStats)); - - // Create mutex - mutex = xSemaphoreCreateRecursiveMutex(); - if (mutex == NULL) - return -1; - - // Initialize default metadata structure (metadata of metaobjects) - defMetadata.access = ACCESS_READWRITE; - defMetadata.gcsAccess = ACCESS_READWRITE; - defMetadata.telemetryAcked = 1; - defMetadata.telemetryUpdateMode = UPDATEMODE_ONCHANGE; - defMetadata.telemetryUpdatePeriod = 0; - defMetadata.gcsTelemetryAcked = 1; - defMetadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE; - defMetadata.gcsTelemetryUpdatePeriod = 0; - defMetadata.loggingUpdateMode = UPDATEMODE_ONCHANGE; - defMetadata.loggingUpdatePeriod = 0; - - // Done - return 0; -} - -/** - * Get the statistics counters - * @param[out] statsOut The statistics counters will be copied there - */ -void UAVObjGetStats(UAVObjStats* statsOut) -{ - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - memcpy(statsOut, &stats, sizeof(UAVObjStats)); - xSemaphoreGiveRecursive(mutex); -} - -/** - * Clear the statistics counters - */ -void UAVObjClearStats() -{ - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - memset(&stats, 0, sizeof(UAVObjStats)); - xSemaphoreGiveRecursive(mutex); -} - -/** - * Register and new object in the object manager. - * \param[in] id Unique object ID - * \param[in] name Object name - * \param[in] nameName Metaobject name - * \param[in] isMetaobject Is this a metaobject (1:true, 0:false) - * \param[in] isSingleInstance Is this a single instance or multi-instance object - * \param[in] isSettings Is this a settings object - * \param[in] numBytes Number of bytes of object data (for one instance) - * \param[in] initCb Default field and metadata initialization function - * \return Object handle, or NULL if failure. - * \return - */ -UAVObjHandle UAVObjRegister(uint32_t id, const char* name, const char* metaName, int32_t isMetaobject, - int32_t isSingleInstance, int32_t isSettings, uint32_t numBytes, UAVObjInitializeCallback initCb) -{ - ObjectList* objEntry; - ObjectInstList* instEntry; - ObjectList* metaObj; - - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Check that the object is not already registered - LL_FOREACH(objList, objEntry) - { - if (objEntry->id == id) - { - // Already registered, ignore - xSemaphoreGiveRecursive(mutex); - return NULL; - } - } - - // Create and append entry - objEntry = (ObjectList*)pvPortMalloc(sizeof(ObjectList)); - if (objEntry == NULL) - { - xSemaphoreGiveRecursive(mutex); - return NULL; - } - objEntry->id = id; - objEntry->name = name; - objEntry->isMetaobject = (int8_t)isMetaobject; - objEntry->isSingleInstance = (int8_t)isSingleInstance; - objEntry->isSettings = (int8_t)isSettings; - objEntry->numBytes = numBytes; - objEntry->events = NULL; - objEntry->numInstances = 0; - objEntry->instances.data = NULL; - objEntry->instances.instId = 0xFFFF; - objEntry->instances.next = NULL; - objEntry->linkedObj = NULL; // will be set later - LL_APPEND(objList, objEntry); - - // Create instance zero - instEntry = createInstance(objEntry, 0); - if ( instEntry == NULL ) - { - xSemaphoreGiveRecursive(mutex); - return NULL; - } - - // Create metaobject and update linkedObj - if (isMetaobject) - { - objEntry->linkedObj = NULL; // will be set later - } - else - { - // Create metaobject - metaObj = (ObjectList*)UAVObjRegister(id+1, metaName, NULL, 1, 1, 0, sizeof(UAVObjMetadata), NULL); - // Link two objects - objEntry->linkedObj = metaObj; - metaObj->linkedObj = objEntry; - } - - // Initialize object fields and metadata to default values - if ( initCb != NULL ) - { - initCb((UAVObjHandle)objEntry, 0); - } - - // Attempt to load object's metadata from the SD card (not done directly on the metaobject, but through the object) - if ( !objEntry->isMetaobject ) - { - UAVObjLoad( (UAVObjHandle)objEntry->linkedObj, 0 ); - } - - // If this is a settings object, attempt to load from SD card - if ( objEntry->isSettings ) - { - UAVObjLoad( (UAVObjHandle)objEntry, 0 ); - } - - // Release lock - xSemaphoreGiveRecursive(mutex); - return (UAVObjHandle)objEntry; -} - -/** - * Retrieve an object from the list given its id - * \param[in] The object ID - * \return The object or NULL if not found. - */ -UAVObjHandle UAVObjGetByID(uint32_t id) -{ - ObjectList* objEntry; - - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Look for object - LL_FOREACH(objList, objEntry) - { - if (objEntry->id == id) - { - // Release lock - xSemaphoreGiveRecursive(mutex); - // Done, object found - return (UAVObjHandle)objEntry; - } - } - - // Object not found, release lock and return error - xSemaphoreGiveRecursive(mutex); - return NULL; -} - -/** - * Retrieve an object from the list given its name - * \param[in] name The name of the object - * \return The object or NULL if not found. - */ -UAVObjHandle UAVObjGetByName(char* name) -{ - ObjectList* objEntry; - - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Look for object - LL_FOREACH(objList, objEntry) - { - if (objEntry->name != NULL && strcmp(objEntry->name, name) == 0) - { - // Release lock - xSemaphoreGiveRecursive(mutex); - // Done, object found - return (UAVObjHandle)objEntry; - } - } - - // Object not found, release lock and return error - xSemaphoreGiveRecursive(mutex); - return NULL; -} - -/** - * Get the object's ID - * \param[in] obj The object handle - * \return The object ID - */ -uint32_t UAVObjGetID(UAVObjHandle obj) -{ - return ((ObjectList*)obj)->id; -} - -/** - * Get the object's name - * \param[in] obj The object handle - * \return The object's name - */ -const char* UAVObjGetName(UAVObjHandle obj) -{ - return ((ObjectList*)obj)->name; -} - -/** - * Get the number of bytes of the object's data (for one instance) - * \param[in] obj The object handle - * \return The number of bytes - */ -uint32_t UAVObjGetNumBytes(UAVObjHandle obj) -{ - return ((ObjectList*)obj)->numBytes; -} - -/** - * Get the object this object is linked to. For regular objects, the linked object - * is the metaobject. For metaobjects the linked object is the parent object. - * This function is normally only needed by the telemetry module. - * \param[in] obj The object handle - * \return The object linked object handle - */ -UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj) -{ - return (UAVObjHandle)(((ObjectList*)obj)->linkedObj); -} - -/** - * Get the number of instances contained in the object. - * \param[in] obj The object handle - * \return The number of instances - */ -uint16_t UAVObjGetNumInstances(UAVObjHandle obj) -{ - uint32_t numInstances; - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - numInstances = ((ObjectList*)obj)->numInstances; - xSemaphoreGiveRecursive(mutex); - return numInstances; -} - -/** - * Create a new instance in the object. - * \param[in] obj The object handle - * \return The instance ID or 0 if an error - */ -uint16_t UAVObjCreateInstance(UAVObjHandle obj, UAVObjInitializeCallback initCb) -{ - ObjectList* objEntry; - ObjectInstList* instEntry; - - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Create new instance - objEntry = (ObjectList*)obj; - instEntry = createInstance(objEntry, objEntry->numInstances); - if ( instEntry == NULL ) - { - xSemaphoreGiveRecursive(mutex); - return -1; - } - - // Initialize instance data - if ( initCb != NULL ) - { - initCb(obj, instEntry->instId); - } - - // Unlock - xSemaphoreGiveRecursive(mutex); - return instEntry->instId; -} - -/** - * Does this object contains a single instance or multiple instances? - * \param[in] obj The object handle - * \return True (1) if this is a single instance object - */ -int32_t UAVObjIsSingleInstance(UAVObjHandle obj) -{ - return ((ObjectList*)obj)->isSingleInstance; -} - -/** - * Is this a metaobject? - * \param[in] obj The object handle - * \return True (1) if this is metaobject - */ -int32_t UAVObjIsMetaobject(UAVObjHandle obj) -{ - return ((ObjectList*)obj)->isMetaobject; -} - -/** - * Is this a settings object? - * \param[in] obj The object handle - * \return True (1) if this is a settings object - */ -int32_t UAVObjIsSettings(UAVObjHandle obj) -{ - return ((ObjectList*)obj)->isSettings; -} - -/** - * Unpack an object from a byte array - * \param[in] obj The object handle - * \param[in] instId The instance ID - * \param[in] dataIn The byte array - * \return 0 if success or -1 if failure - */ -int32_t UAVObjUnpack(UAVObjHandle obj, uint16_t instId, const uint8_t* dataIn) -{ - ObjectList* objEntry; - ObjectInstList* instEntry; - - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Cast handle to object - objEntry = (ObjectList*)obj; - - // Get the instance - instEntry = getInstance(objEntry, instId); - - // If the instance does not exist create it and any other instances before it - if ( instEntry == NULL ) - { - instEntry = createInstance(objEntry, instId); - if ( instEntry == NULL ) - { - // Error, unlock and return - xSemaphoreGiveRecursive(mutex); - return -1; - } - } - - // Set the data - memcpy(instEntry->data, dataIn, objEntry->numBytes); - - // Fire event - sendEvent(objEntry, instId, EV_UNPACKED); - - // Unlock - xSemaphoreGiveRecursive(mutex); - return 0; -} - -/** - * Pack an object to a byte array - * \param[in] obj The object handle - * \param[in] instId The instance ID - * \param[out] dataOut The byte array - * \return 0 if success or -1 if failure - */ -int32_t UAVObjPack(UAVObjHandle obj, uint16_t instId, uint8_t* dataOut) -{ - ObjectList* objEntry; - ObjectInstList* instEntry; - - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Cast handle to object - objEntry = (ObjectList*)obj; - - // Get the instance - instEntry = getInstance(objEntry, instId); - if ( instEntry == NULL ) - { - // Error, unlock and return - xSemaphoreGiveRecursive(mutex); - return -1; - } - - // Pack data - memcpy(dataOut, instEntry->data, objEntry->numBytes); - - // Unlock - xSemaphoreGiveRecursive(mutex); - return 0; -} - -/** - * Save the data of the specified object instance to the file system (SD card). - * The object will be appended and the file will not be closed. - * The object data can be restored using the UAVObjLoad function. - * @param[in] obj The object handle. - * @param[in] instId The instance ID - * @param[in] file File to append to - * @return 0 if success or -1 if failure - */ -int32_t UAVObjSaveToFile(UAVObjHandle obj, uint16_t instId, FILEINFO* file) -{ -#if defined(PIOS_INCLUDE_SDCARD) - uint32_t bytesWritten; - ObjectList* objEntry; - ObjectInstList* instEntry; - - // Check for file system availability - if ( PIOS_SDCARD_IsMounted() == 0 ) - { - return -1; - } - - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Cast to object - objEntry = (ObjectList*)obj; - - // Get the instance information - instEntry = getInstance(objEntry, instId); - if ( instEntry == NULL ) - { - xSemaphoreGiveRecursive(mutex); - return -1; - } - - // Write the object ID - PIOS_FWRITE(file,&objEntry->id,sizeof(objEntry->id),&bytesWritten); - - // Write the instance ID - if (!objEntry->isSingleInstance) - { - PIOS_FWRITE(file,&instEntry->instId,sizeof(instEntry->instId),&bytesWritten); - } - - // Write the data and check that the write was successful - PIOS_FWRITE(file,instEntry->data,objEntry->numBytes,&bytesWritten); - if ( bytesWritten != objEntry->numBytes ) - { - xSemaphoreGiveRecursive(mutex); - return -1; - } - - // Done - xSemaphoreGiveRecursive(mutex); -#endif /* PIOS_INCLUDE_SDCARD */ - return 0; -} - -struct fileHeader { - uint32_t id; - uint16_t instId; - uint16_t size; -} __attribute__((packed)); - -#define FLASH_MASK 0x001ff000 /* Select a sector */ - -/** - * Save the data of the specified object to the file system (SD card). - * If the object contains multiple instances, all of them will be saved. - * A new file with the name of the object will be created. - * The object data can be restored using the UAVObjLoad function. - * @param[in] obj The object handle. - * @param[in] instId The instance ID - * @param[in] file File to append to - * @return 0 if success or -1 if failure - */ -int32_t UAVObjSave(UAVObjHandle obj, uint16_t instId) -{ -#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS) - ObjectList* objEntry = (ObjectList*)obj; - - if(objEntry == NULL) - return -1; - - ObjectInstList* instEntry = getInstance(objEntry, instId); - - if(instEntry == NULL) - return -1; - - if(instEntry->data == NULL) - return -1; - - - struct fileHeader header = { - .id = objEntry->id, - .instId = instId, - .size = objEntry->numBytes - }; - - uint32_t addr = (objEntry->id & FLASH_MASK); - PIOS_Flash_W25X_EraseSector(addr); - PIOS_Flash_W25X_WriteData(addr, (uint8_t *) &header, sizeof(header)); - PIOS_Flash_W25X_WriteData(addr + sizeof(header), instEntry->data,objEntry->numBytes); -#endif -#if defined(PIOS_INCLUDE_SDCARD) - FILEINFO file; - ObjectList* objEntry; - uint8_t filename[14]; - - // Check for file system availability - if ( PIOS_SDCARD_IsMounted() == 0 ) - { - return -1; - } - - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Cast to object - objEntry = (ObjectList*)obj; - - // Get filename - objectFilename(objEntry, filename); - - // Open file - if ( PIOS_FOPEN_WRITE(filename,file) ) - { - xSemaphoreGiveRecursive(mutex); - return -1; - } - - // Append object - if ( UAVObjSaveToFile(obj, instId, &file) == -1 ) - { - PIOS_FCLOSE(file); - xSemaphoreGiveRecursive(mutex); - return -1; - } - - // Done, close file and unlock - PIOS_FCLOSE(file); - xSemaphoreGiveRecursive(mutex); -#endif /* PIOS_INCLUDE_SDCARD */ - return 0; -} - -/** - * Load an object from the file system (SD card). - * @param[in] file File to read from - * @return The handle of the object loaded or NULL if a failure - */ -UAVObjHandle UAVObjLoadFromFile(FILEINFO* file) -{ -#if defined(PIOS_INCLUDE_SDCARD) - uint32_t bytesRead; - ObjectList* objEntry; - ObjectInstList* instEntry; - uint32_t objId; - uint16_t instId; - UAVObjHandle obj; - - // Check for file system availability - if ( PIOS_SDCARD_IsMounted() == 0 ) - { - return NULL; - } - - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Read the object ID - if ( PIOS_FREAD(file,&objId,sizeof(objId),&bytesRead) ) - { - xSemaphoreGiveRecursive(mutex); - return NULL; - } - - // Get the object - obj = UAVObjGetByID(objId); - if ( obj == 0 ) - { - xSemaphoreGiveRecursive(mutex); - return NULL; - } - objEntry = (ObjectList*)obj; - - // Get the instance ID - instId = 0; - if ( !objEntry->isSingleInstance ) - { - if ( PIOS_FREAD(file,&instId,sizeof(instId),&bytesRead) ) - { - xSemaphoreGiveRecursive(mutex); - return NULL; - } - } - - // Get the instance information - instEntry = getInstance(objEntry, instId); - - // If the instance does not exist create it and any other instances before it - if ( instEntry == NULL ) - { - instEntry = createInstance(objEntry, instId); - if ( instEntry == NULL ) - { - // Error, unlock and return - xSemaphoreGiveRecursive(mutex); - return NULL; - } - } - - // Read the instance data - if ( PIOS_FREAD(file,instEntry->data,objEntry->numBytes,&bytesRead) ) - { - xSemaphoreGiveRecursive(mutex); - return NULL; - } - - // Fire event - sendEvent(objEntry, instId, EV_UNPACKED); - - // Unlock - xSemaphoreGiveRecursive(mutex); - return obj; -#else /* PIOS_INCLUDE_SDCARD */ - return NULL; -#endif -} - -/** - * Load an object from the file system (SD card). - * A file with the name of the object will be opened. - * The object data can be saved using the UAVObjSave function. - * @param[in] obj The object handle. - * @param[in] instId The object instance - * @return 0 if success or -1 if failure - */ -int32_t UAVObjLoad(UAVObjHandle obj, uint16_t instId) -{ -#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS) - ObjectList* objEntry = (ObjectList*)obj; - - if(objEntry == NULL) - return -1; - - ObjectInstList* instEntry = getInstance(objEntry, instId); - - if(instEntry == NULL) - return -1; - - if(instEntry->data == NULL) - return -1; - - struct fileHeader header; - uint32_t addr = (objEntry->id & FLASH_MASK); - - PIOS_Flash_W25X_ReadData(addr, (uint8_t *) &header, sizeof(header)); - - if(header.id != objEntry->id) - return -1; - - // Read the instance data - if (PIOS_Flash_W25X_ReadData(addr + sizeof(header) ,instEntry->data, objEntry->numBytes) != 0) - return -1; - - // Fire event - sendEvent(objEntry, instId, EV_UNPACKED); -#endif - -#if defined(PIOS_INCLUDE_SDCARD) - FILEINFO file; - ObjectList* objEntry; - UAVObjHandle loadedObj; - ObjectList* loadedObjEntry; - uint8_t filename[14]; - - // Check for file system availability - if ( PIOS_SDCARD_IsMounted() == 0 ) - { - return -1; - } - - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Cast to object - objEntry = (ObjectList*)obj; - - // Get filename - objectFilename(objEntry, filename); - - // Open file - if ( PIOS_FOPEN_READ(filename,file) ) - { - xSemaphoreGiveRecursive(mutex); - return -1; - } - - // Load object - loadedObj = UAVObjLoadFromFile(&file); - if (loadedObj == 0) - { - PIOS_FCLOSE(file); - xSemaphoreGiveRecursive(mutex); - return -1; - } - - // Check that the IDs match - loadedObjEntry = (ObjectList*)loadedObj; - if ( loadedObjEntry->id != objEntry->id ) - { - PIOS_FCLOSE(file); - xSemaphoreGiveRecursive(mutex); - return -1; - } - - // Done, close file and unlock - PIOS_FCLOSE(file); - xSemaphoreGiveRecursive(mutex); -#endif /* PIOS_INCLUDE_SDCARD */ - return 0; -} - -/** - * Delete an object from the file system (SD card). - * @param[in] obj The object handle. - * @param[in] instId The object instance - * @return 0 if success or -1 if failure - */ -int32_t UAVObjDelete(UAVObjHandle obj, uint16_t instId) -{ -#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS) - ObjectList* objEntry = (ObjectList*)obj; - - if(objEntry == NULL) - return -1; - - uint32_t addr = (objEntry->id & FLASH_MASK); - PIOS_Flash_W25X_EraseSector(addr); -#endif -#if defined(PIOS_INCLUDE_SDCARD) - ObjectList* objEntry; - uint8_t filename[14]; - - // Check for file system availability - if ( PIOS_SDCARD_IsMounted() == 0 ) - { - return -1; - } - - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Cast to object - objEntry = (ObjectList*)obj; - - // Get filename - objectFilename(objEntry, filename); - - // Delete file - PIOS_FUNLINK(filename); - - // Done - xSemaphoreGiveRecursive(mutex); -#endif /* PIOS_INCLUDE_SDCARD */ - return 0; -} - -/** - * Save all settings objects to the SD card. - * @return 0 if success or -1 if failure - */ -int32_t UAVObjSaveSettings() -{ - ObjectList* objEntry; - - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Save all settings objects - LL_FOREACH(objList, objEntry) - { - // Check if this is a settings object - if ( objEntry->isSettings ) - { - // Save object - if ( UAVObjSave( (UAVObjHandle)objEntry, 0 ) == -1 ) - { - xSemaphoreGiveRecursive(mutex); - return -1; - } - } - } - - // Done - xSemaphoreGiveRecursive(mutex); - return 0; -} - -/** - * Load all settings objects from the SD card. - * @return 0 if success or -1 if failure - */ -int32_t UAVObjLoadSettings() -{ - ObjectList* objEntry; - - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Load all settings objects - LL_FOREACH(objList, objEntry) - { - // Check if this is a settings object - if ( objEntry->isSettings ) - { - // Load object - if ( UAVObjLoad( (UAVObjHandle)objEntry, 0 ) == -1 ) - { - xSemaphoreGiveRecursive(mutex); - return -1; - } - } - } - - // Done - xSemaphoreGiveRecursive(mutex); - return 0; -} - -/** - * Delete all settings objects from the SD card. - * @return 0 if success or -1 if failure - */ -int32_t UAVObjDeleteSettings() -{ - ObjectList* objEntry; - - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Save all settings objects - LL_FOREACH(objList, objEntry) - { - // Check if this is a settings object - if ( objEntry->isSettings ) - { - // Save object - if ( UAVObjDelete( (UAVObjHandle)objEntry, 0 ) == -1 ) - { - xSemaphoreGiveRecursive(mutex); - return -1; - } - } - } - - // Done - xSemaphoreGiveRecursive(mutex); - return 0; -} - -/** - * Save all metaobjects to the SD card. - * @return 0 if success or -1 if failure - */ -int32_t UAVObjSaveMetaobjects() -{ - ObjectList* objEntry; - - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Save all settings objects - LL_FOREACH(objList, objEntry) - { - // Check if this is a settings object - if ( objEntry->isMetaobject ) - { - // Save object - if ( UAVObjSave( (UAVObjHandle)objEntry, 0 ) == -1 ) - { - xSemaphoreGiveRecursive(mutex); - return -1; - } - } - } - - // Done - xSemaphoreGiveRecursive(mutex); - return 0; -} - -/** - * Load all metaobjects from the SD card. - * @return 0 if success or -1 if failure - */ -int32_t UAVObjLoadMetaobjects() -{ - ObjectList* objEntry; - - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Load all settings objects - LL_FOREACH(objList, objEntry) - { - // Check if this is a settings object - if ( objEntry->isMetaobject ) - { - // Load object - if ( UAVObjLoad( (UAVObjHandle)objEntry, 0 ) == -1 ) - { - xSemaphoreGiveRecursive(mutex); - return -1; - } - } - } - - // Done - xSemaphoreGiveRecursive(mutex); - return 0; -} - -/** - * Delete all metaobjects from the SD card. - * @return 0 if success or -1 if failure - */ -int32_t UAVObjDeleteMetaobjects() -{ - ObjectList* objEntry; - - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Load all settings objects - LL_FOREACH(objList, objEntry) - { - // Check if this is a settings object - if ( objEntry->isMetaobject ) - { - // Load object - if ( UAVObjDelete( (UAVObjHandle)objEntry, 0 ) == -1 ) - { - xSemaphoreGiveRecursive(mutex); - return -1; - } - } - } - - // Done - xSemaphoreGiveRecursive(mutex); - return 0; -} - -/** - * Set the object data - * \param[in] obj The object handle - * \param[in] dataIn The object's data structure - * \return 0 if success or -1 if failure - */ -int32_t UAVObjSetData(UAVObjHandle obj, const void* dataIn) -{ - return UAVObjSetInstanceData(obj, 0, dataIn); -} - -/** - * Get the object data - * \param[in] obj The object handle - * \param[out] dataOut The object's data structure - * \return 0 if success or -1 if failure - */ -int32_t UAVObjGetData(UAVObjHandle obj, void* dataOut) -{ - return UAVObjGetInstanceData(obj, 0, dataOut); -} - -/** - * Set the data of a specific object instance - * \param[in] obj The object handle - * \param[in] instId The object instance ID - * \param[in] dataIn The object's data structure - * \return 0 if success or -1 if failure - */ -int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint16_t instId, const void* dataIn) -{ - ObjectList* objEntry; - ObjectInstList* instEntry; - UAVObjMetadata* mdata; - - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Cast to object info - objEntry = (ObjectList*)obj; - - // Check access level - if ( !objEntry->isMetaobject ) - { - mdata = (UAVObjMetadata*)(objEntry->linkedObj->instances.data); - if ( mdata->access == ACCESS_READONLY ) - { - xSemaphoreGiveRecursive(mutex); - return -1; - } - } - - // Get instance information - instEntry = getInstance(objEntry, instId); - if ( instEntry == NULL ) - { - // Error, unlock and return - xSemaphoreGiveRecursive(mutex); - return -1; - } - - // Set data - memcpy(instEntry->data, dataIn, objEntry->numBytes); - - // Fire event - sendEvent(objEntry, instId, EV_UPDATED); - - // Unlock - xSemaphoreGiveRecursive(mutex); - return 0; -} - -/** - * Get the data of a specific object instance - * \param[in] obj The object handle - * \param[in] instId The object instance ID - * \param[out] dataOut The object's data structure - * \return 0 if success or -1 if failure - */ -int32_t UAVObjGetInstanceData(UAVObjHandle obj, uint16_t instId, void* dataOut) -{ - ObjectList* objEntry; - ObjectInstList* instEntry; - - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Cast to object info - objEntry = (ObjectList*)obj; - - // Get instance information - instEntry = getInstance(objEntry, instId); - if ( instEntry == NULL ) - { - // Error, unlock and return - xSemaphoreGiveRecursive(mutex); - return -1; - } - - // Set data - memcpy(dataOut, instEntry->data, objEntry->numBytes); - - // Unlock - xSemaphoreGiveRecursive(mutex); - return 0; -} - -/** - * Set the object metadata - * \param[in] obj The object handle - * \param[in] dataIn The object's metadata structure - * \return 0 if success or -1 if failure - */ -int32_t UAVObjSetMetadata(UAVObjHandle obj, const UAVObjMetadata* dataIn) -{ - ObjectList* objEntry; - - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Set metadata (metadata of metaobjects can not be modified) - objEntry = (ObjectList*)obj; - if (!objEntry->isMetaobject) - { - UAVObjSetData((UAVObjHandle)objEntry->linkedObj, dataIn); - } - else - { - return -1; - } - - // Unlock - xSemaphoreGiveRecursive(mutex); - return 0; -} - -/** - * Get the object metadata - * \param[in] obj The object handle - * \param[out] dataOut The object's metadata structure - * \return 0 if success or -1 if failure - */ -int32_t UAVObjGetMetadata(UAVObjHandle obj, UAVObjMetadata* dataOut) -{ - ObjectList* objEntry; - - // Lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Get metadata - objEntry = (ObjectList*)obj; - if (objEntry->isMetaobject) - { - memcpy(dataOut, &defMetadata, sizeof(UAVObjMetadata)); - } - else - { - UAVObjGetData((UAVObjHandle)objEntry->linkedObj, dataOut); - } - - // Unlock - xSemaphoreGiveRecursive(mutex); - return 0; -} - -/** - * Check if an object is read only - * \param[in] obj The object handle - * \return - * \arg 0 if not read only - * \arg 1 if read only - * \arg -1 if unable to get meta data - */ -int8_t UAVObjReadOnly(UAVObjHandle obj) -{ - ObjectList* objEntry; - UAVObjMetadata* mdata; - - // Cast to object info - objEntry = (ObjectList*)obj; - - // Check access level - if ( !objEntry->isMetaobject ) - { - mdata = (UAVObjMetadata*)(objEntry->linkedObj->instances.data); - return mdata->access == ACCESS_READONLY; - } - return -1; -} - -/** - * Connect an event queue to the object, if the queue is already connected then the event mask is only updated. - * All events matching the event mask will be pushed to the event queue. - * \param[in] obj The object handle - * \param[in] queue The event queue - * \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL) - * \return 0 if success or -1 if failure - */ -int32_t UAVObjConnectQueue(UAVObjHandle obj, xQueueHandle queue, int32_t eventMask) -{ - int32_t res; - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - res = connectObj(obj, queue, 0, eventMask); - xSemaphoreGiveRecursive(mutex); - return res; -} - -/** - * Disconnect an event queue from the object. - * \param[in] obj The object handle - * \param[in] queue The event queue - * \return 0 if success or -1 if failure - */ -int32_t UAVObjDisconnectQueue(UAVObjHandle obj, xQueueHandle queue) -{ - int32_t res; - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - res = disconnectObj(obj, queue, 0); - xSemaphoreGiveRecursive(mutex); - return res; -} - -/** - * Connect an event callback to the object, if the callback is already connected then the event mask is only updated. - * The supplied callback will be invoked on all events matching the event mask. - * \param[in] obj The object handle - * \param[in] cb The event callback - * \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL) - * \return 0 if success or -1 if failure - */ -int32_t UAVObjConnectCallback(UAVObjHandle obj, UAVObjEventCallback cb, int32_t eventMask) -{ - int32_t res; - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - res = connectObj(obj, 0, cb, eventMask); - xSemaphoreGiveRecursive(mutex); - return res; -} - -/** - * Disconnect an event callback from the object. - * \param[in] obj The object handle - * \param[in] cb The event callback - * \return 0 if success or -1 if failure - */ -int32_t UAVObjDisconnectCallback(UAVObjHandle obj, UAVObjEventCallback cb) -{ - int32_t res; - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - res = disconnectObj(obj, 0, cb); - xSemaphoreGiveRecursive(mutex); - return res; -} - - -/** - * Request an update of the object's data from the GCS. The call will not wait for the response, a EV_UPDATED event - * will be generated as soon as the object is updated. - * \param[in] obj The object handle - */ -void UAVObjRequestUpdate(UAVObjHandle obj) -{ - UAVObjRequestInstanceUpdate(obj, UAVOBJ_ALL_INSTANCES); -} - -/** - * Request an update of the object's data from the GCS. The call will not wait for the response, a EV_UPDATED event - * will be generated as soon as the object is updated. - * \param[in] obj The object handle - * \param[in] instId Object instance ID to update - */ -void UAVObjRequestInstanceUpdate(UAVObjHandle obj, uint16_t instId) -{ - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - sendEvent((ObjectList*)obj, instId, EV_UPDATE_REQ); - xSemaphoreGiveRecursive(mutex); -} - -/** - * Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object). - * \param[in] obj The object handle - */ -void UAVObjUpdated(UAVObjHandle obj) -{ - UAVObjInstanceUpdated(obj, UAVOBJ_ALL_INSTANCES); -} - -/** - * Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object). - * \param[in] obj The object handle - * \param[in] instId The object instance ID - */ -void UAVObjInstanceUpdated(UAVObjHandle obj, uint16_t instId) -{ - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - sendEvent((ObjectList*)obj, instId, EV_UPDATED_MANUAL); - xSemaphoreGiveRecursive(mutex); -} - -/** - * Iterate through all objects in the list. - * \param iterator This function will be called once for each object, - * the object will be passed as a parameter - */ -void UAVObjIterate(void (*iterator)(UAVObjHandle obj)) -{ - ObjectList* objEntry; - - // Get lock - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - - // Iterate through the list and invoke iterator for each object - LL_FOREACH(objList, objEntry) - { - (*iterator)((UAVObjHandle)objEntry); - } - - // Release lock - xSemaphoreGiveRecursive(mutex); -} - -/** - * Send an event to all event queues registered on the object. - */ -static int32_t sendEvent(ObjectList* obj, uint16_t instId, UAVObjEventType event) -{ - ObjectEventList* eventEntry; - UAVObjEvent msg; - - // Setup event - msg.obj = (UAVObjHandle)obj; - msg.event = event; - msg.instId = instId; - - // Go through each object and push the event message in the queue (if event is activated for the queue) - LL_FOREACH(obj->events, eventEntry) - { - if ( eventEntry->eventMask == 0 || (eventEntry->eventMask & event) != 0 ) - { - // Send to queue if a valid queue is registered - if (eventEntry->queue != 0) - { - if ( xQueueSend(eventEntry->queue, &msg, 0) != pdTRUE ) // will not block - { - ++stats.eventErrors; - } - } - // Invoke callback (from event task) if a valid one is registered - if (eventEntry->cb != 0) - { - if ( EventCallbackDispatch(&msg, eventEntry->cb) != pdTRUE ) // invoke callback from the event task, will not block - { - ++stats.eventErrors; - } - } - } - } - - // Done - return 0; -} - -/** - * Create a new object instance, return the instance info or NULL if failure. - */ -static ObjectInstList* createInstance(ObjectList* obj, uint16_t instId) -{ - ObjectInstList* instEntry; - int32_t n; - - // For single instance objects, only instance zero is allowed - if (obj->isSingleInstance && instId != 0) - { - return NULL; - } - - // Make sure that the instance ID is within limits - if (instId >= UAVOBJ_MAX_INSTANCES) - { - return NULL; - } - - // Check if the instance already exists - if ( getInstance(obj, instId) != NULL ) - { - return NULL; - } - - // Create any missing instances (all instance IDs must be sequential) - for (n = obj->numInstances; n < instId; ++n) - { - if ( createInstance(obj, n) == NULL ) - { - return NULL; - } - } - - if(instId == 0) /* Instance 0 ObjectInstList allocated with ObjectList element */ - { - instEntry = &obj->instances; - instEntry->data = pvPortMalloc(obj->numBytes); - if (instEntry->data == NULL) return NULL; - memset(instEntry->data, 0, obj->numBytes); - instEntry->instId = instId; - } else - { - // Create the actual instance - instEntry = (ObjectInstList*)pvPortMalloc(sizeof(ObjectInstList)); - if (instEntry == NULL) return NULL; - instEntry->data = pvPortMalloc(obj->numBytes); - if (instEntry->data == NULL) return NULL; - memset(instEntry->data, 0, obj->numBytes); - instEntry->instId = instId; - LL_APPEND(obj->instances.next, instEntry); - } - ++obj->numInstances; - - // Fire event - UAVObjInstanceUpdated((UAVObjHandle)obj, instId); - - // Done - return instEntry; -} - -/** - * Get the instance information or NULL if the instance does not exist - */ -static ObjectInstList* getInstance(ObjectList* obj, uint16_t instId) -{ - ObjectInstList* instEntry; - - // Look for specified instance ID - LL_FOREACH(&(obj->instances), instEntry) - { - if (instEntry->instId == instId) - { - return instEntry; - } - } - // If this point is reached then instance id was not found - return NULL; -} - -/** - * Connect an event queue to the object, if the queue is already connected then the event mask is only updated. - * \param[in] obj The object handle - * \param[in] queue The event queue - * \param[in] cb The event callback - * \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL) - * \return 0 if success or -1 if failure - */ -static int32_t connectObj(UAVObjHandle obj, xQueueHandle queue, UAVObjEventCallback cb, int32_t eventMask) -{ - ObjectEventList* eventEntry; - ObjectList* objEntry; - - // Check that the queue is not already connected, if it is simply update event mask - objEntry = (ObjectList*)obj; - LL_FOREACH(objEntry->events, eventEntry) - { - if ( eventEntry->queue == queue && eventEntry->cb == cb ) - { - // Already connected, update event mask and return - eventEntry->eventMask = eventMask; - return 0; - } - } - - // Add queue to list - eventEntry = (ObjectEventList*)pvPortMalloc(sizeof(ObjectEventList)); - if (eventEntry == NULL) - { - return -1; - } - eventEntry->queue = queue; - eventEntry->cb = cb; - eventEntry->eventMask = eventMask; - LL_APPEND(objEntry->events, eventEntry); - - // Done - return 0; -} - -/** - * Disconnect an event queue from the object - * \param[in] obj The object handle - * \param[in] queue The event queue - * \param[in] cb The event callback - * \return 0 if success or -1 if failure - */ -static int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue, UAVObjEventCallback cb) -{ - ObjectEventList* eventEntry; - ObjectList* objEntry; - - // Find queue and remove it - objEntry = (ObjectList*)obj; - LL_FOREACH(objEntry->events, eventEntry) - { - if ( ( eventEntry->queue == queue && eventEntry->cb == cb ) ) - { - LL_DELETE(objEntry->events, eventEntry); - vPortFree(eventEntry); - return 0; - } - } - - // If this point is reached the queue was not found - return -1; -} - -#if defined(PIOS_INCLUDE_SDCARD) -/** - * Wrapper for the sprintf function - */ -static void customSPrintf(uint8_t* buffer, uint8_t* format, ...) -{ - va_list args; - va_start(args, format); - vsprintf((char *)buffer, (char *)format, args); -} - -/** - * Get an 8 character (plus extension) filename for the object. - */ -static void objectFilename(ObjectList* obj, uint8_t* filename) -{ - customSPrintf(filename, (uint8_t*)"%X.obj", obj->id); -} -#endif /* PIOS_INCLUDE_SDCARD */ - - - - - - - - - - - - - - - - +/** + ****************************************************************************** + * @addtogroup UAVObjects OpenPilot UAVObjects + * @{ + * @addtogroup UAV Object Manager + * @brief The core UAV Objects functions, most of which are wrappered by + * autogenerated defines + * @{ + * + * + * @file uavobjectmanager.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Object manager library. This library holds a collection of all objects. + * It can be used by all modules/libraries to find an object reference. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "openpilot.h" + +// Constants + +// Private types + +/** + * List of event queues and the eventmask associated with the queue. + */ +struct ObjectEventListStruct { + xQueueHandle queue; + UAVObjEventCallback cb; + int32_t eventMask; + struct ObjectEventListStruct *next; +}; +typedef struct ObjectEventListStruct ObjectEventList; + +/** + * List of object instances, holds the actual data structure and instance ID + */ +struct ObjectInstListStruct { + void *data; + uint16_t instId; + struct ObjectInstListStruct *next; +}; +typedef struct ObjectInstListStruct ObjectInstList; + +/** + * List of objects registered in the object manager + */ +struct ObjectListStruct { + uint32_t id; + /** The object ID */ + const char *name; + /** The object name */ + int8_t isMetaobject; + /** Set to 1 if this is a metaobject */ + int8_t isSingleInstance; + /** Set to 1 if this object has a single instance */ + int8_t isSettings; + /** Set to 1 if this object is a settings object */ + uint16_t numBytes; + /** Number of data bytes contained in the object (for a single instance) */ + uint16_t numInstances; + /** Number of instances */ + struct ObjectListStruct *linkedObj; + /** Linked object, for regular objects this is the metaobject and for metaobjects it is the parent object */ + ObjectInstList instances; + /** List of object instances, instance 0 always exists */ + ObjectEventList *events; + /** Event queues registered on the object */ + struct ObjectListStruct *next; + /** Needed by linked list library (utlist.h) */ +}; +typedef struct ObjectListStruct ObjectList; + +// Private functions +static int32_t sendEvent(ObjectList * obj, uint16_t instId, + UAVObjEventType event); +static ObjectInstList *createInstance(ObjectList * obj, uint16_t instId); +static ObjectInstList *getInstance(ObjectList * obj, uint16_t instId); +static int32_t connectObj(UAVObjHandle obj, xQueueHandle queue, + UAVObjEventCallback cb, int32_t eventMask); +static int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue, + UAVObjEventCallback cb); + +#if defined(PIOS_INCLUDE_SDCARD) +static void objectFilename(ObjectList * obj, uint8_t * filename); +static void customSPrintf(uint8_t * buffer, uint8_t * format, ...); +#endif + +// Private variables +static ObjectList *objList; +static xSemaphoreHandle mutex; +static UAVObjMetadata defMetadata; +static UAVObjStats stats; + +/** + * Initialize the object manager + * \return 0 Success + * \return -1 Failure + */ +int32_t UAVObjInitialize() +{ + // Initialize variables + objList = NULL; + memset(&stats, 0, sizeof(UAVObjStats)); + + // Create mutex + mutex = xSemaphoreCreateRecursiveMutex(); + if (mutex == NULL) + return -1; + + // Initialize default metadata structure (metadata of metaobjects) + defMetadata.access = ACCESS_READWRITE; + defMetadata.gcsAccess = ACCESS_READWRITE; + defMetadata.telemetryAcked = 1; + defMetadata.telemetryUpdateMode = UPDATEMODE_ONCHANGE; + defMetadata.telemetryUpdatePeriod = 0; + defMetadata.gcsTelemetryAcked = 1; + defMetadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE; + defMetadata.gcsTelemetryUpdatePeriod = 0; + defMetadata.loggingUpdateMode = UPDATEMODE_ONCHANGE; + defMetadata.loggingUpdatePeriod = 0; + + // Done + return 0; +} + +/** + * Get the statistics counters + * @param[out] statsOut The statistics counters will be copied there + */ +void UAVObjGetStats(UAVObjStats * statsOut) +{ + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + memcpy(statsOut, &stats, sizeof(UAVObjStats)); + xSemaphoreGiveRecursive(mutex); +} + +/** + * Clear the statistics counters + */ +void UAVObjClearStats() +{ + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + memset(&stats, 0, sizeof(UAVObjStats)); + xSemaphoreGiveRecursive(mutex); +} + +/** + * Register and new object in the object manager. + * \param[in] id Unique object ID + * \param[in] name Object name + * \param[in] nameName Metaobject name + * \param[in] isMetaobject Is this a metaobject (1:true, 0:false) + * \param[in] isSingleInstance Is this a single instance or multi-instance object + * \param[in] isSettings Is this a settings object + * \param[in] numBytes Number of bytes of object data (for one instance) + * \param[in] initCb Default field and metadata initialization function + * \return Object handle, or NULL if failure. + * \return + */ +UAVObjHandle UAVObjRegister(uint32_t id, const char *name, + const char *metaName, int32_t isMetaobject, + int32_t isSingleInstance, int32_t isSettings, + uint32_t numBytes, + UAVObjInitializeCallback initCb) +{ + ObjectList *objEntry; + ObjectInstList *instEntry; + ObjectList *metaObj; + + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Check that the object is not already registered + LL_FOREACH(objList, objEntry) { + if (objEntry->id == id) { + // Already registered, ignore + xSemaphoreGiveRecursive(mutex); + return NULL; + } + } + + // Create and append entry + objEntry = (ObjectList *) pvPortMalloc(sizeof(ObjectList)); + if (objEntry == NULL) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } + objEntry->id = id; + objEntry->name = name; + objEntry->isMetaobject = (int8_t) isMetaobject; + objEntry->isSingleInstance = (int8_t) isSingleInstance; + objEntry->isSettings = (int8_t) isSettings; + objEntry->numBytes = numBytes; + objEntry->events = NULL; + objEntry->numInstances = 0; + objEntry->instances.data = NULL; + objEntry->instances.instId = 0xFFFF; + objEntry->instances.next = NULL; + objEntry->linkedObj = NULL; // will be set later + LL_APPEND(objList, objEntry); + + // Create instance zero + instEntry = createInstance(objEntry, 0); + if (instEntry == NULL) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } + // Create metaobject and update linkedObj + if (isMetaobject) { + objEntry->linkedObj = NULL; // will be set later + } else { + // Create metaobject + metaObj = + (ObjectList *) UAVObjRegister(id + 1, metaName, + NULL, 1, 1, 0, + sizeof + (UAVObjMetadata), + NULL); + // Link two objects + objEntry->linkedObj = metaObj; + metaObj->linkedObj = objEntry; + } + + // Initialize object fields and metadata to default values + if (initCb != NULL) { + initCb((UAVObjHandle) objEntry, 0); + } + // Attempt to load object's metadata from the SD card (not done directly on the metaobject, but through the object) + if (!objEntry->isMetaobject) { + UAVObjLoad((UAVObjHandle) objEntry->linkedObj, 0); + } + // If this is a settings object, attempt to load from SD card + if (objEntry->isSettings) { + UAVObjLoad((UAVObjHandle) objEntry, 0); + } + // Release lock + xSemaphoreGiveRecursive(mutex); + return (UAVObjHandle) objEntry; +} + +/** + * Retrieve an object from the list given its id + * \param[in] The object ID + * \return The object or NULL if not found. + */ +UAVObjHandle UAVObjGetByID(uint32_t id) +{ + ObjectList *objEntry; + + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Look for object + LL_FOREACH(objList, objEntry) { + if (objEntry->id == id) { + // Release lock + xSemaphoreGiveRecursive(mutex); + // Done, object found + return (UAVObjHandle) objEntry; + } + } + + // Object not found, release lock and return error + xSemaphoreGiveRecursive(mutex); + return NULL; +} + +/** + * Retrieve an object from the list given its name + * \param[in] name The name of the object + * \return The object or NULL if not found. + */ +UAVObjHandle UAVObjGetByName(char *name) +{ + ObjectList *objEntry; + + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Look for object + LL_FOREACH(objList, objEntry) { + if (objEntry->name != NULL + && strcmp(objEntry->name, name) == 0) { + // Release lock + xSemaphoreGiveRecursive(mutex); + // Done, object found + return (UAVObjHandle) objEntry; + } + } + + // Object not found, release lock and return error + xSemaphoreGiveRecursive(mutex); + return NULL; +} + +/** + * Get the object's ID + * \param[in] obj The object handle + * \return The object ID + */ +uint32_t UAVObjGetID(UAVObjHandle obj) +{ + return ((ObjectList *) obj)->id; +} + +/** + * Get the object's name + * \param[in] obj The object handle + * \return The object's name + */ +const char *UAVObjGetName(UAVObjHandle obj) +{ + return ((ObjectList *) obj)->name; +} + +/** + * Get the number of bytes of the object's data (for one instance) + * \param[in] obj The object handle + * \return The number of bytes + */ +uint32_t UAVObjGetNumBytes(UAVObjHandle obj) +{ + return ((ObjectList *) obj)->numBytes; +} + +/** + * Get the object this object is linked to. For regular objects, the linked object + * is the metaobject. For metaobjects the linked object is the parent object. + * This function is normally only needed by the telemetry module. + * \param[in] obj The object handle + * \return The object linked object handle + */ +UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj) +{ + return (UAVObjHandle) (((ObjectList *) obj)->linkedObj); +} + +/** + * Get the number of instances contained in the object. + * \param[in] obj The object handle + * \return The number of instances + */ +uint16_t UAVObjGetNumInstances(UAVObjHandle obj) +{ + uint32_t numInstances; + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + numInstances = ((ObjectList *) obj)->numInstances; + xSemaphoreGiveRecursive(mutex); + return numInstances; +} + +/** + * Create a new instance in the object. + * \param[in] obj The object handle + * \return The instance ID or 0 if an error + */ +uint16_t UAVObjCreateInstance(UAVObjHandle obj, + UAVObjInitializeCallback initCb) +{ + ObjectList *objEntry; + ObjectInstList *instEntry; + + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Create new instance + objEntry = (ObjectList *) obj; + instEntry = createInstance(objEntry, objEntry->numInstances); + if (instEntry == NULL) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Initialize instance data + if (initCb != NULL) { + initCb(obj, instEntry->instId); + } + // Unlock + xSemaphoreGiveRecursive(mutex); + return instEntry->instId; +} + +/** + * Does this object contains a single instance or multiple instances? + * \param[in] obj The object handle + * \return True (1) if this is a single instance object + */ +int32_t UAVObjIsSingleInstance(UAVObjHandle obj) +{ + return ((ObjectList *) obj)->isSingleInstance; +} + +/** + * Is this a metaobject? + * \param[in] obj The object handle + * \return True (1) if this is metaobject + */ +int32_t UAVObjIsMetaobject(UAVObjHandle obj) +{ + return ((ObjectList *) obj)->isMetaobject; +} + +/** + * Is this a settings object? + * \param[in] obj The object handle + * \return True (1) if this is a settings object + */ +int32_t UAVObjIsSettings(UAVObjHandle obj) +{ + return ((ObjectList *) obj)->isSettings; +} + +/** + * Unpack an object from a byte array + * \param[in] obj The object handle + * \param[in] instId The instance ID + * \param[in] dataIn The byte array + * \return 0 if success or -1 if failure + */ +int32_t UAVObjUnpack(UAVObjHandle obj, uint16_t instId, + const uint8_t * dataIn) +{ + ObjectList *objEntry; + ObjectInstList *instEntry; + + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Cast handle to object + objEntry = (ObjectList *) obj; + + // Get the instance + instEntry = getInstance(objEntry, instId); + + // If the instance does not exist create it and any other instances before it + if (instEntry == NULL) { + instEntry = createInstance(objEntry, instId); + if (instEntry == NULL) { + // Error, unlock and return + xSemaphoreGiveRecursive(mutex); + return -1; + } + } + // Set the data + memcpy(instEntry->data, dataIn, objEntry->numBytes); + + // Fire event + sendEvent(objEntry, instId, EV_UNPACKED); + + // Unlock + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * Pack an object to a byte array + * \param[in] obj The object handle + * \param[in] instId The instance ID + * \param[out] dataOut The byte array + * \return 0 if success or -1 if failure + */ +int32_t UAVObjPack(UAVObjHandle obj, uint16_t instId, uint8_t * dataOut) +{ + ObjectList *objEntry; + ObjectInstList *instEntry; + + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Cast handle to object + objEntry = (ObjectList *) obj; + + // Get the instance + instEntry = getInstance(objEntry, instId); + if (instEntry == NULL) { + // Error, unlock and return + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Pack data + memcpy(dataOut, instEntry->data, objEntry->numBytes); + + // Unlock + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * Save the data of the specified object instance to the file system (SD card). + * The object will be appended and the file will not be closed. + * The object data can be restored using the UAVObjLoad function. + * @param[in] obj The object handle. + * @param[in] instId The instance ID + * @param[in] file File to append to + * @return 0 if success or -1 if failure + */ +int32_t UAVObjSaveToFile(UAVObjHandle obj, uint16_t instId, + FILEINFO * file) +{ +#if defined(PIOS_INCLUDE_SDCARD) + uint32_t bytesWritten; + ObjectList *objEntry; + ObjectInstList *instEntry; + + // Check for file system availability + if (PIOS_SDCARD_IsMounted() == 0) { + return -1; + } + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Cast to object + objEntry = (ObjectList *) obj; + + // Get the instance information + instEntry = getInstance(objEntry, instId); + if (instEntry == NULL) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Write the object ID + PIOS_FWRITE(file, &objEntry->id, sizeof(objEntry->id), + &bytesWritten); + + // Write the instance ID + if (!objEntry->isSingleInstance) { + PIOS_FWRITE(file, &instEntry->instId, + sizeof(instEntry->instId), &bytesWritten); + } + // Write the data and check that the write was successful + PIOS_FWRITE(file, instEntry->data, objEntry->numBytes, + &bytesWritten); + if (bytesWritten != objEntry->numBytes) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Done + xSemaphoreGiveRecursive(mutex); +#endif /* PIOS_INCLUDE_SDCARD */ + return 0; +} + +/** + * Save the data of the specified object to the file system (SD card). + * If the object contains multiple instances, all of them will be saved. + * A new file with the name of the object will be created. + * The object data can be restored using the UAVObjLoad function. + * @param[in] obj The object handle. + * @param[in] instId The instance ID + * @param[in] file File to append to + * @return 0 if success or -1 if failure + */ +int32_t UAVObjSave(UAVObjHandle obj, uint16_t instId) +{ +#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS) + ObjectList *objEntry = (ObjectList *) obj; + + if (objEntry == NULL) + return -1; + + ObjectInstList *instEntry = getInstance(objEntry, instId); + + if (instEntry == NULL) + return -1; + + if (instEntry->data == NULL) + return -1; + + if (PIOS_FLASHFS_ObjSave(obj, instId, instEntry->data) != 0) + return -1; +#endif +#if defined(PIOS_INCLUDE_SDCARD) + FILEINFO file; + ObjectList *objEntry; + uint8_t filename[14]; + + // Check for file system availability + if (PIOS_SDCARD_IsMounted() == 0) { + return -1; + } + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Cast to object + objEntry = (ObjectList *) obj; + + // Get filename + objectFilename(objEntry, filename); + + // Open file + if (PIOS_FOPEN_WRITE(filename, file)) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Append object + if (UAVObjSaveToFile(obj, instId, &file) == -1) { + PIOS_FCLOSE(file); + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Done, close file and unlock + PIOS_FCLOSE(file); + xSemaphoreGiveRecursive(mutex); +#endif /* PIOS_INCLUDE_SDCARD */ + return 0; +} + +/** + * Load an object from the file system (SD card). + * @param[in] file File to read from + * @return The handle of the object loaded or NULL if a failure + */ +UAVObjHandle UAVObjLoadFromFile(FILEINFO * file) +{ +#if defined(PIOS_INCLUDE_SDCARD) + uint32_t bytesRead; + ObjectList *objEntry; + ObjectInstList *instEntry; + uint32_t objId; + uint16_t instId; + UAVObjHandle obj; + + // Check for file system availability + if (PIOS_SDCARD_IsMounted() == 0) { + return NULL; + } + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Read the object ID + if (PIOS_FREAD(file, &objId, sizeof(objId), &bytesRead)) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } + // Get the object + obj = UAVObjGetByID(objId); + if (obj == 0) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } + objEntry = (ObjectList *) obj; + + // Get the instance ID + instId = 0; + if (!objEntry->isSingleInstance) { + if (PIOS_FREAD + (file, &instId, sizeof(instId), &bytesRead)) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } + } + // Get the instance information + instEntry = getInstance(objEntry, instId); + + // If the instance does not exist create it and any other instances before it + if (instEntry == NULL) { + instEntry = createInstance(objEntry, instId); + if (instEntry == NULL) { + // Error, unlock and return + xSemaphoreGiveRecursive(mutex); + return NULL; + } + } + // Read the instance data + if (PIOS_FREAD + (file, instEntry->data, objEntry->numBytes, &bytesRead)) { + xSemaphoreGiveRecursive(mutex); + return NULL; + } + // Fire event + sendEvent(objEntry, instId, EV_UNPACKED); + + // Unlock + xSemaphoreGiveRecursive(mutex); + return obj; +#else /* PIOS_INCLUDE_SDCARD */ + return NULL; +#endif +} + +/** + * Load an object from the file system (SD card). + * A file with the name of the object will be opened. + * The object data can be saved using the UAVObjSave function. + * @param[in] obj The object handle. + * @param[in] instId The object instance + * @return 0 if success or -1 if failure + */ +int32_t UAVObjLoad(UAVObjHandle obj, uint16_t instId) +{ +#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS) + ObjectList *objEntry = (ObjectList *) obj; + + if (objEntry == NULL) + return -1; + + ObjectInstList *instEntry = getInstance(objEntry, instId); + + if (instEntry == NULL) + return -1; + + if (instEntry->data == NULL) + return -1; + + // Fire event on success + if (PIOS_FLASHFS_ObjLoad(obj, instId, instEntry->data) == 0) + sendEvent(objEntry, instId, EV_UNPACKED); + else + return -1; +#endif + +#if defined(PIOS_INCLUDE_SDCARD) + FILEINFO file; + ObjectList *objEntry; + UAVObjHandle loadedObj; + ObjectList *loadedObjEntry; + uint8_t filename[14]; + + // Check for file system availability + if (PIOS_SDCARD_IsMounted() == 0) { + return -1; + } + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Cast to object + objEntry = (ObjectList *) obj; + + // Get filename + objectFilename(objEntry, filename); + + // Open file + if (PIOS_FOPEN_READ(filename, file)) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Load object + loadedObj = UAVObjLoadFromFile(&file); + if (loadedObj == 0) { + PIOS_FCLOSE(file); + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Check that the IDs match + loadedObjEntry = (ObjectList *) loadedObj; + if (loadedObjEntry->id != objEntry->id) { + PIOS_FCLOSE(file); + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Done, close file and unlock + PIOS_FCLOSE(file); + xSemaphoreGiveRecursive(mutex); +#endif /* PIOS_INCLUDE_SDCARD */ + return 0; +} + +/** + * Delete an object from the file system (SD card). + * @param[in] obj The object handle. + * @param[in] instId The object instance + * @return 0 if success or -1 if failure + */ +int32_t UAVObjDelete(UAVObjHandle obj, uint16_t instId) +{ +#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS) + PIOS_FLASHFS_ObjDelete(obj, instId); +#endif +#if defined(PIOS_INCLUDE_SDCARD) + ObjectList *objEntry; + uint8_t filename[14]; + + // Check for file system availability + if (PIOS_SDCARD_IsMounted() == 0) { + return -1; + } + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Cast to object + objEntry = (ObjectList *) obj; + + // Get filename + objectFilename(objEntry, filename); + + // Delete file + PIOS_FUNLINK(filename); + + // Done + xSemaphoreGiveRecursive(mutex); +#endif /* PIOS_INCLUDE_SDCARD */ + return 0; +} + +/** + * Save all settings objects to the SD card. + * @return 0 if success or -1 if failure + */ +int32_t UAVObjSaveSettings() +{ + ObjectList *objEntry; + + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Save all settings objects + LL_FOREACH(objList, objEntry) { + // Check if this is a settings object + if (objEntry->isSettings) { + // Save object + if (UAVObjSave((UAVObjHandle) objEntry, 0) == + -1) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + } + } + + // Done + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * Load all settings objects from the SD card. + * @return 0 if success or -1 if failure + */ +int32_t UAVObjLoadSettings() +{ + ObjectList *objEntry; + + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Load all settings objects + LL_FOREACH(objList, objEntry) { + // Check if this is a settings object + if (objEntry->isSettings) { + // Load object + if (UAVObjLoad((UAVObjHandle) objEntry, 0) == + -1) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + } + } + + // Done + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * Delete all settings objects from the SD card. + * @return 0 if success or -1 if failure + */ +int32_t UAVObjDeleteSettings() +{ + ObjectList *objEntry; + + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Save all settings objects + LL_FOREACH(objList, objEntry) { + // Check if this is a settings object + if (objEntry->isSettings) { + // Save object + if (UAVObjDelete((UAVObjHandle) objEntry, 0) + == -1) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + } + } + + // Done + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * Save all metaobjects to the SD card. + * @return 0 if success or -1 if failure + */ +int32_t UAVObjSaveMetaobjects() +{ + ObjectList *objEntry; + + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Save all settings objects + LL_FOREACH(objList, objEntry) { + // Check if this is a settings object + if (objEntry->isMetaobject) { + // Save object + if (UAVObjSave((UAVObjHandle) objEntry, 0) == + -1) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + } + } + + // Done + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * Load all metaobjects from the SD card. + * @return 0 if success or -1 if failure + */ +int32_t UAVObjLoadMetaobjects() +{ + ObjectList *objEntry; + + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Load all settings objects + LL_FOREACH(objList, objEntry) { + // Check if this is a settings object + if (objEntry->isMetaobject) { + // Load object + if (UAVObjLoad((UAVObjHandle) objEntry, 0) == + -1) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + } + } + + // Done + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * Delete all metaobjects from the SD card. + * @return 0 if success or -1 if failure + */ +int32_t UAVObjDeleteMetaobjects() +{ + ObjectList *objEntry; + + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Load all settings objects + LL_FOREACH(objList, objEntry) { + // Check if this is a settings object + if (objEntry->isMetaobject) { + // Load object + if (UAVObjDelete((UAVObjHandle) objEntry, 0) + == -1) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + } + } + + // Done + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * Set the object data + * \param[in] obj The object handle + * \param[in] dataIn The object's data structure + * \return 0 if success or -1 if failure + */ +int32_t UAVObjSetData(UAVObjHandle obj, const void *dataIn) +{ + return UAVObjSetInstanceData(obj, 0, dataIn); +} + +/** + * Get the object data + * \param[in] obj The object handle + * \param[out] dataOut The object's data structure + * \return 0 if success or -1 if failure + */ +int32_t UAVObjGetData(UAVObjHandle obj, void *dataOut) +{ + return UAVObjGetInstanceData(obj, 0, dataOut); +} + +/** + * Set the data of a specific object instance + * \param[in] obj The object handle + * \param[in] instId The object instance ID + * \param[in] dataIn The object's data structure + * \return 0 if success or -1 if failure + */ +int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint16_t instId, + const void *dataIn) +{ + ObjectList *objEntry; + ObjectInstList *instEntry; + UAVObjMetadata *mdata; + + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Cast to object info + objEntry = (ObjectList *) obj; + + // Check access level + if (!objEntry->isMetaobject) { + mdata = + (UAVObjMetadata *) (objEntry->linkedObj->instances. + data); + if (mdata->access == ACCESS_READONLY) { + xSemaphoreGiveRecursive(mutex); + return -1; + } + } + // Get instance information + instEntry = getInstance(objEntry, instId); + if (instEntry == NULL) { + // Error, unlock and return + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Set data + memcpy(instEntry->data, dataIn, objEntry->numBytes); + + // Fire event + sendEvent(objEntry, instId, EV_UPDATED); + + // Unlock + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * Get the data of a specific object instance + * \param[in] obj The object handle + * \param[in] instId The object instance ID + * \param[out] dataOut The object's data structure + * \return 0 if success or -1 if failure + */ +int32_t UAVObjGetInstanceData(UAVObjHandle obj, uint16_t instId, + void *dataOut) +{ + ObjectList *objEntry; + ObjectInstList *instEntry; + + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Cast to object info + objEntry = (ObjectList *) obj; + + // Get instance information + instEntry = getInstance(objEntry, instId); + if (instEntry == NULL) { + // Error, unlock and return + xSemaphoreGiveRecursive(mutex); + return -1; + } + // Set data + memcpy(dataOut, instEntry->data, objEntry->numBytes); + + // Unlock + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * Set the object metadata + * \param[in] obj The object handle + * \param[in] dataIn The object's metadata structure + * \return 0 if success or -1 if failure + */ +int32_t UAVObjSetMetadata(UAVObjHandle obj, const UAVObjMetadata * dataIn) +{ + ObjectList *objEntry; + + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Set metadata (metadata of metaobjects can not be modified) + objEntry = (ObjectList *) obj; + if (!objEntry->isMetaobject) { + UAVObjSetData((UAVObjHandle) objEntry->linkedObj, + dataIn); + } else { + return -1; + } + + // Unlock + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * Get the object metadata + * \param[in] obj The object handle + * \param[out] dataOut The object's metadata structure + * \return 0 if success or -1 if failure + */ +int32_t UAVObjGetMetadata(UAVObjHandle obj, UAVObjMetadata * dataOut) +{ + ObjectList *objEntry; + + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Get metadata + objEntry = (ObjectList *) obj; + if (objEntry->isMetaobject) { + memcpy(dataOut, &defMetadata, sizeof(UAVObjMetadata)); + } else { + UAVObjGetData((UAVObjHandle) objEntry->linkedObj, + dataOut); + } + + // Unlock + xSemaphoreGiveRecursive(mutex); + return 0; +} + +/** + * Check if an object is read only + * \param[in] obj The object handle + * \return + * \arg 0 if not read only + * \arg 1 if read only + * \arg -1 if unable to get meta data + */ +int8_t UAVObjReadOnly(UAVObjHandle obj) +{ + ObjectList *objEntry; + UAVObjMetadata *mdata; + + // Cast to object info + objEntry = (ObjectList *) obj; + + // Check access level + if (!objEntry->isMetaobject) { + mdata = + (UAVObjMetadata *) (objEntry->linkedObj->instances. + data); + return mdata->access == ACCESS_READONLY; + } + return -1; +} + +/** + * Connect an event queue to the object, if the queue is already connected then the event mask is only updated. + * All events matching the event mask will be pushed to the event queue. + * \param[in] obj The object handle + * \param[in] queue The event queue + * \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL) + * \return 0 if success or -1 if failure + */ +int32_t UAVObjConnectQueue(UAVObjHandle obj, xQueueHandle queue, + int32_t eventMask) +{ + int32_t res; + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + res = connectObj(obj, queue, 0, eventMask); + xSemaphoreGiveRecursive(mutex); + return res; +} + +/** + * Disconnect an event queue from the object. + * \param[in] obj The object handle + * \param[in] queue The event queue + * \return 0 if success or -1 if failure + */ +int32_t UAVObjDisconnectQueue(UAVObjHandle obj, xQueueHandle queue) +{ + int32_t res; + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + res = disconnectObj(obj, queue, 0); + xSemaphoreGiveRecursive(mutex); + return res; +} + +/** + * Connect an event callback to the object, if the callback is already connected then the event mask is only updated. + * The supplied callback will be invoked on all events matching the event mask. + * \param[in] obj The object handle + * \param[in] cb The event callback + * \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL) + * \return 0 if success or -1 if failure + */ +int32_t UAVObjConnectCallback(UAVObjHandle obj, UAVObjEventCallback cb, + int32_t eventMask) +{ + int32_t res; + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + res = connectObj(obj, 0, cb, eventMask); + xSemaphoreGiveRecursive(mutex); + return res; +} + +/** + * Disconnect an event callback from the object. + * \param[in] obj The object handle + * \param[in] cb The event callback + * \return 0 if success or -1 if failure + */ +int32_t UAVObjDisconnectCallback(UAVObjHandle obj, UAVObjEventCallback cb) +{ + int32_t res; + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + res = disconnectObj(obj, 0, cb); + xSemaphoreGiveRecursive(mutex); + return res; +} + +/** + * Request an update of the object's data from the GCS. The call will not wait for the response, a EV_UPDATED event + * will be generated as soon as the object is updated. + * \param[in] obj The object handle + */ +void UAVObjRequestUpdate(UAVObjHandle obj) +{ + UAVObjRequestInstanceUpdate(obj, UAVOBJ_ALL_INSTANCES); +} + +/** + * Request an update of the object's data from the GCS. The call will not wait for the response, a EV_UPDATED event + * will be generated as soon as the object is updated. + * \param[in] obj The object handle + * \param[in] instId Object instance ID to update + */ +void UAVObjRequestInstanceUpdate(UAVObjHandle obj, uint16_t instId) +{ + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + sendEvent((ObjectList *) obj, instId, EV_UPDATE_REQ); + xSemaphoreGiveRecursive(mutex); +} + +/** + * Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object). + * \param[in] obj The object handle + */ +void UAVObjUpdated(UAVObjHandle obj) +{ + UAVObjInstanceUpdated(obj, UAVOBJ_ALL_INSTANCES); +} + +/** + * Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object). + * \param[in] obj The object handle + * \param[in] instId The object instance ID + */ +void UAVObjInstanceUpdated(UAVObjHandle obj, uint16_t instId) +{ + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + sendEvent((ObjectList *) obj, instId, EV_UPDATED_MANUAL); + xSemaphoreGiveRecursive(mutex); +} + +/** + * Iterate through all objects in the list. + * \param iterator This function will be called once for each object, + * the object will be passed as a parameter + */ +void UAVObjIterate(void (*iterator) (UAVObjHandle obj)) +{ + ObjectList *objEntry; + + // Get lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Iterate through the list and invoke iterator for each object + LL_FOREACH(objList, objEntry) { + (*iterator) ((UAVObjHandle) objEntry); + } + + // Release lock + xSemaphoreGiveRecursive(mutex); +} + +/** + * Send an event to all event queues registered on the object. + */ +static int32_t sendEvent(ObjectList * obj, uint16_t instId, + UAVObjEventType event) +{ + ObjectEventList *eventEntry; + UAVObjEvent msg; + + // Setup event + msg.obj = (UAVObjHandle) obj; + msg.event = event; + msg.instId = instId; + + // Go through each object and push the event message in the queue (if event is activated for the queue) + LL_FOREACH(obj->events, eventEntry) { + if (eventEntry->eventMask == 0 + || (eventEntry->eventMask & event) != 0) { + // Send to queue if a valid queue is registered + if (eventEntry->queue != 0) { + if (xQueueSend(eventEntry->queue, &msg, 0) != pdTRUE) // will not block + { + ++stats.eventErrors; + } + } + // Invoke callback (from event task) if a valid one is registered + if (eventEntry->cb != 0) { + if (EventCallbackDispatch(&msg, eventEntry->cb) != pdTRUE) // invoke callback from the event task, will not block + { + ++stats.eventErrors; + } + } + } + } + + // Done + return 0; +} + +/** + * Create a new object instance, return the instance info or NULL if failure. + */ +static ObjectInstList *createInstance(ObjectList * obj, uint16_t instId) +{ + ObjectInstList *instEntry; + int32_t n; + + // For single instance objects, only instance zero is allowed + if (obj->isSingleInstance && instId != 0) { + return NULL; + } + // Make sure that the instance ID is within limits + if (instId >= UAVOBJ_MAX_INSTANCES) { + return NULL; + } + // Check if the instance already exists + if (getInstance(obj, instId) != NULL) { + return NULL; + } + // Create any missing instances (all instance IDs must be sequential) + for (n = obj->numInstances; n < instId; ++n) { + if (createInstance(obj, n) == NULL) { + return NULL; + } + } + + if (instId == 0) { /* Instance 0 ObjectInstList allocated with ObjectList element */ + instEntry = &obj->instances; + instEntry->data = pvPortMalloc(obj->numBytes); + if (instEntry->data == NULL) + return NULL; + memset(instEntry->data, 0, obj->numBytes); + instEntry->instId = instId; + } else { + // Create the actual instance + instEntry = + (ObjectInstList *) + pvPortMalloc(sizeof(ObjectInstList)); + if (instEntry == NULL) + return NULL; + instEntry->data = pvPortMalloc(obj->numBytes); + if (instEntry->data == NULL) + return NULL; + memset(instEntry->data, 0, obj->numBytes); + instEntry->instId = instId; + LL_APPEND(obj->instances.next, instEntry); + } + ++obj->numInstances; + + // Fire event + UAVObjInstanceUpdated((UAVObjHandle) obj, instId); + + // Done + return instEntry; +} + +/** + * Get the instance information or NULL if the instance does not exist + */ +static ObjectInstList *getInstance(ObjectList * obj, uint16_t instId) +{ + ObjectInstList *instEntry; + + // Look for specified instance ID + LL_FOREACH(&(obj->instances), instEntry) { + if (instEntry->instId == instId) { + return instEntry; + } + } + // If this point is reached then instance id was not found + return NULL; +} + +/** + * Connect an event queue to the object, if the queue is already connected then the event mask is only updated. + * \param[in] obj The object handle + * \param[in] queue The event queue + * \param[in] cb The event callback + * \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL) + * \return 0 if success or -1 if failure + */ +static int32_t connectObj(UAVObjHandle obj, xQueueHandle queue, + UAVObjEventCallback cb, int32_t eventMask) +{ + ObjectEventList *eventEntry; + ObjectList *objEntry; + + // Check that the queue is not already connected, if it is simply update event mask + objEntry = (ObjectList *) obj; + LL_FOREACH(objEntry->events, eventEntry) { + if (eventEntry->queue == queue && eventEntry->cb == cb) { + // Already connected, update event mask and return + eventEntry->eventMask = eventMask; + return 0; + } + } + + // Add queue to list + eventEntry = + (ObjectEventList *) pvPortMalloc(sizeof(ObjectEventList)); + if (eventEntry == NULL) { + return -1; + } + eventEntry->queue = queue; + eventEntry->cb = cb; + eventEntry->eventMask = eventMask; + LL_APPEND(objEntry->events, eventEntry); + + // Done + return 0; +} + +/** + * Disconnect an event queue from the object + * \param[in] obj The object handle + * \param[in] queue The event queue + * \param[in] cb The event callback + * \return 0 if success or -1 if failure + */ +static int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue, + UAVObjEventCallback cb) +{ + ObjectEventList *eventEntry; + ObjectList *objEntry; + + // Find queue and remove it + objEntry = (ObjectList *) obj; + LL_FOREACH(objEntry->events, eventEntry) { + if ((eventEntry->queue == queue + && eventEntry->cb == cb)) { + LL_DELETE(objEntry->events, eventEntry); + vPortFree(eventEntry); + return 0; + } + } + + // If this point is reached the queue was not found + return -1; +} + +#if defined(PIOS_INCLUDE_SDCARD) +/** + * Wrapper for the sprintf function + */ +static void customSPrintf(uint8_t * buffer, uint8_t * format, ...) +{ + va_list args; + va_start(args, format); + vsprintf((char *)buffer, (char *)format, args); +} + +/** + * Get an 8 character (plus extension) filename for the object. + */ +static void objectFilename(ObjectList * obj, uint8_t * filename) +{ + customSPrintf(filename, (uint8_t *) "%X.obj", obj->id); +} +#endif /* PIOS_INCLUDE_SDCARD */