自学教程：C++ xSemaphoreGiveRecursive函数代码示例

/** * 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 UAVObjGetInstanceDataField(UAVObjHandle obj, uint16_t instId, void* dataOut, uint32_t offset, uint32_t size){	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;	}	// return if we request too much of what we can give	if ( (size + offset) > objEntry->numBytes) 	{		// Error, unlock and return		xSemaphoreGiveRecursive(mutex);		return -1;	}		// Set data	memcpy(dataOut, instEntry->data + offset, size);	// 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;}

/** * 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;}

/** * Check if there are any alarms with the given or higher severity * @return 0 if no alarms are found, 1 if at least one alarm is found */static int32_t hasSeverity(SystemAlarmsAlarmOptions severity){	SystemAlarmsData alarms;	uint32_t n;	// Lock    xSemaphoreTakeRecursive(lock, portMAX_DELAY);    // Read alarms    SystemAlarmsGet(&alarms);    // Go through alarms and check if any are of the given severity or higher    for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n)    {    	if ( alarms.Alarm[n] >= severity)    	{    		xSemaphoreGiveRecursive(lock);    		return 1;    	}    }    // If this point is reached then no alarms found    xSemaphoreGiveRecursive(lock);    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;}

/** * 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;}

static void prvExerciseSemaphoreAPI( void ){SemaphoreHandle_t xSemaphore;const UBaseType_t uxMaxCount = 5, uxInitialCount = 0;	/* Most of the semaphore API is common to the queue API and is already being	used.  This function uses a few semaphore functions that are unique to the	RTOS objects, rather than generic and used by queues also.	First create and use a counting semaphore. */	xSemaphore = xSemaphoreCreateCounting( uxMaxCount, uxInitialCount );	configASSERT( xSemaphore );	/* Give the semaphore a couple of times and ensure the count is returned	correctly. */	xSemaphoreGive( xSemaphore );	xSemaphoreGive( xSemaphore );	configASSERT( uxSemaphoreGetCount( xSemaphore ) == 2 );	vSemaphoreDelete( xSemaphore );	/* Create a recursive mutex, and ensure the mutex holder and count are	returned returned correctly. */	xSemaphore = xSemaphoreCreateRecursiveMutex();	configASSERT( uxSemaphoreGetCount( xSemaphore ) == 1 );	configASSERT( xSemaphore );	xSemaphoreTakeRecursive( xSemaphore, mainDONT_BLOCK );	xSemaphoreTakeRecursive( xSemaphore, mainDONT_BLOCK );	configASSERT( xSemaphoreGetMutexHolder( xSemaphore ) == xTaskGetCurrentTaskHandle() );	configASSERT( xSemaphoreGetMutexHolder( xSemaphore ) == xTaskGetHandle( mainTASK_TO_DELETE_NAME ) );	xSemaphoreGiveRecursive( xSemaphore );	configASSERT( uxSemaphoreGetCount( xSemaphore ) == 0 );	xSemaphoreGiveRecursive( xSemaphore );	configASSERT( uxSemaphoreGetCount( xSemaphore ) == 1 );	configASSERT( xSemaphoreGetMutexHolder( xSemaphore ) == NULL );	vSemaphoreDelete( xSemaphore );	/* Create a normal mutex, and sure the mutex holder and count are returned	returned correctly. */	xSemaphore = xSemaphoreCreateMutex();	configASSERT( xSemaphore );	xSemaphoreTake( xSemaphore, mainDONT_BLOCK );	xSemaphoreTake( xSemaphore, mainDONT_BLOCK );	configASSERT( uxSemaphoreGetCount( xSemaphore ) == 0 ); /* Not recursive so can only be 1. */	configASSERT( xSemaphoreGetMutexHolder( xSemaphore ) == xTaskGetCurrentTaskHandle() );	xSemaphoreGive( xSemaphore );	configASSERT( uxSemaphoreGetCount( xSemaphore ) == 1 );	configASSERT( xSemaphoreGetMutexHolder( xSemaphore ) == NULL );	vSemaphoreDelete( xSemaphore );}

/** * 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 UAVObjSetInstanceDataField(UAVObjHandle obj, uint16_t instId, const void* dataIn, uint32_t offset, uint32_t size){	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;	}	// return if we set too much of what we have	if ( (size + offset) > objEntry->numBytes) {		// Error, unlock and return		xSemaphoreGiveRecursive(mutex);				return -1;	}	// Set data	memcpy(instEntry->data + offset, dataIn, size);	// Fire event	sendEvent(objEntry, instId, EV_UPDATED);	// Unlock	xSemaphoreGiveRecursive(mutex);	return 0;}

/** * Set an alarm * @param alarm The system alarm to be modified * @param severity The alarm severity * @return 0 if success, -1 if an error */int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity){	SystemAlarmsData alarms;	// Check that this is a valid alarm	if (alarm >= SYSTEMALARMS_ALARM_NUMELEM)	{		return -1;	}	// Lock    xSemaphoreTakeRecursive(lock, portMAX_DELAY);    // Read alarm and update its severity only if it was changed    SystemAlarmsGet(&alarms);    if ( alarms.Alarm[alarm] != severity )    {    	alarms.Alarm[alarm] = severity;    	SystemAlarmsSet(&alarms);    }    // Release lock    xSemaphoreGiveRecursive(lock);    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;}

/** * Update the status of all tasks */void TaskMonitorUpdateAll(void){	TaskInfoData data;	int n;	// Lock	xSemaphoreTakeRecursive(lock, portMAX_DELAY);	// Update all task information	for (n = 0; n < TASKINFO_RUNNING_NUMELEM; ++n)	{		if (handles[n] != 0)		{			data.Running[n] = TASKINFO_RUNNING_TRUE;#if defined(ARCH_POSIX) || defined(ARCH_WIN32)			data.StackRemaining[n] = 10000;#else			data.StackRemaining[n] = uxTaskGetStackHighWaterMark(handles[n]) * 4;#endif		}		else		{			data.Running[n] = TASKINFO_RUNNING_FALSE;			data.StackRemaining[n] = 0;		}	}	// Update object	TaskInfoSet(&data);	// Done	xSemaphoreGiveRecursive(lock);}

int simpleQueueSize(struct SimpleQueue* queue){	int return_value;	while(xSemaphoreTakeRecursive(queue->xSemHandle,(TickType_t)0) != pdTRUE){}	return_value = queue->size;	xSemaphoreGiveRecursive(queue->xSemHandle);	return return_value;}

/** * Get communication statistics counters * /param[in] connection UAVTalkConnection to be used * @param[out] statsOut Statistics counters */void UAVTalkAddStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut, bool reset){    UAVTalkConnectionData *connection;    CHECKCONHANDLE(connectionHandle, connection, return );    // Lock    xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);    // Copy stats    statsOut->txBytes       += connection->stats.txBytes;    statsOut->txObjectBytes += connection->stats.txObjectBytes;    statsOut->txObjects     += connection->stats.txObjects;    statsOut->txErrors      += connection->stats.txErrors;    statsOut->rxBytes       += connection->stats.rxBytes;    statsOut->rxObjectBytes += connection->stats.rxObjectBytes;    statsOut->rxObjects     += connection->stats.rxObjects;    statsOut->rxErrors      += connection->stats.rxErrors;    statsOut->rxSyncErrors  += connection->stats.rxSyncErrors;    statsOut->rxCrcErrors   += connection->stats.rxCrcErrors;    if (reset) {        // Clear stats        memset(&connection->stats, 0, sizeof(UAVTalkStats));    }    // Release lock    xSemaphoreGiveRecursive(connection->lock);}

void simpleQueueDestroy(struct SimpleQueue* queue){	while(xSemaphoreTakeRecursive(queue->xSemHandle,(TickType_t) 0)!=pdTRUE){}	while(!simpleQueueEmpty(queue))		simpleQueueDequeue(queue);	 xSemaphoreGiveRecursive(queue->xSemHandle);	 vSemaphoreDelete(queue->xSemHandle);	//TODO: ZUPPA}

/** * Execute the requested transaction on an object. * /param[in] connection UAVLinkConnection to be used * /param[in] obj Object * /param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. * /param[in] type Transaction type * 			  UAVLINK_TYPE_OBJ: send object, * 			  UAVLINK_TYPE_OBJ_REQ: request object update * 			  UAVLINK_TYPE_OBJ_ACK: send object with an ack * /return 0 Success * /return -1 Failure */static int32_t objectTransaction(UAVLinkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type, int32_t timeoutMs){	int32_t respReceived;		// Send object depending on if a response is needed	if (type == UAVLINK_TYPE_OBJ_ACK || type == UAVLINK_TYPE_OBJ_REQ)	{		// Get transaction lock (will block if a transaction is pending)		xSemaphoreTakeRecursive(connection->transLock, portMAX_DELAY);		// Send object		xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);		connection->respId =  UAVObjGetID(obj);		connection->respInstId = instId;		sendObject(connection, obj, instId, type);		xSemaphoreGiveRecursive(connection->lock);		// Wait for response (or timeout)		respReceived = xSemaphoreTake(connection->respSema, timeoutMs/portTICK_RATE_MS);		// Check if a response was received		if (respReceived == pdFALSE)		{			// Cancel transaction			xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);			xSemaphoreTake(connection->respSema, 0); // non blocking call to make sure the value is reset to zero (binary sema)			connection->respId = 0;			xSemaphoreGiveRecursive(connection->lock);			xSemaphoreGiveRecursive(connection->transLock);			return -1;		}		else		{			xSemaphoreGiveRecursive(connection->transLock);			return 0;		}	}	else if (type == UAVLINK_TYPE_OBJ)	{		xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);		sendObject(connection, obj, instId, UAVLINK_TYPE_OBJ);		xSemaphoreGiveRecursive(connection->lock);		return 0;	}	else	{		return -1;	}}

/** * 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;}

 void __malloc_unlock(struct _reent *reent){     if ( g_heap_mutex ){         if ( pdFALSE == xSemaphoreGiveRecursive( g_heap_mutex)){             Serial2.println("BUG! failed to unlock the heap!");             delay(1000);         }     } }

void __malloc_unlock(struct _reent *ptr){    UNUSED_PARAMETER( ptr );    if( malloc_mutex )    {        xSemaphoreGiveRecursive( malloc_mutex );    }}

/** * 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;}

static void prvSanityCheckCreatedRecursiveMutex( SemaphoreHandle_t xSemaphore ){const BaseType_t xLoops = 5;BaseType_t x, xReturned;	/* A very basic test that the recursive semaphore behaved like a recursive	semaphore. First the semaphore should not be able to be given, as it has not	yet been taken. */	xReturned = xSemaphoreGiveRecursive( xSemaphore );	if( xReturned != pdFAIL )	{		xErrorOccurred = pdTRUE;	}	/* Now it should be possible to take the mutex a number of times. */	for( x = 0; x < xLoops; x++ )	{		xReturned = xSemaphoreTakeRecursive( xSemaphore, staticDONT_BLOCK );		if( xReturned != pdPASS )		{			xErrorOccurred = pdTRUE;		}	}	/* Should be possible to give the semaphore the same number of times as it	was given in the loop above. */	for( x = 0; x < xLoops; x++ )	{		xReturned = xSemaphoreGiveRecursive( xSemaphore );		if( xReturned != pdPASS )		{			xErrorOccurred = pdTRUE;		}	}	/* No more gives should be possible though. */	xReturned = xSemaphoreGiveRecursive( xSemaphore );	if( xReturned != pdFAIL )	{		xErrorOccurred = pdTRUE;	}}

/** * 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;}

void dispatch_queue::dispatch_thread_handler(void){	BaseType_t status = xSemaphoreTakeRecursive(mutex_, portMAX_DELAY);	assert(status == pdTRUE && "Failed to lock mutex!");	do {		//after wait, we own the lock		if(!quit_ && q_.size())		{			auto op = std::move(q_.front());			q_.pop();			//unlock now that we're done messing with the queue			status = xSemaphoreGiveRecursive(mutex_);			assert(status == pdTRUE && "Failed to unlock mutex!");			op();			status = xSemaphoreTakeRecursive(mutex_, portMAX_DELAY);			assert(status == pdTRUE && "Failed to lock mutex!");		}		else if(!quit_)		{			status = xSemaphoreGiveRecursive(mutex_);			assert(status == pdTRUE && "Failed to unlock mutex!");			// Wait for new work - clear flags on exit			xEventGroupWaitBits(notify_flags_, DISPATCH_WAKE_EVT, pdTRUE, pdFALSE, portMAX_DELAY);			status = xSemaphoreTakeRecursive(mutex_, portMAX_DELAY);			assert(status == pdTRUE && "Failed to lock mutex!");		}	} while (!quit_);	// We were holding the mutex after we woke up	status = xSemaphoreGiveRecursive(mutex_);	assert(status == pdTRUE && "Failed to unlock mutex!");	// Set a signal to indicate a thread exited	status = xEventGroupSetBits(notify_flags_, DISPATCH_EXIT_EVT);	assert(status == pdTRUE && "Failed to set event flags!");	// Delete the current thread	vTaskDelete(NULL);}

/** * 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;}

void __env_unlock ( struct _reent *_r ){#if OS_THREAD_SAFE_NEWLIB	if (!xTaskGetSchedulerState())		return;	  	xSemaphoreGiveRecursive(alt_envsem);#endif /* OS_THREAD_SAFE_NEWLIB */}

static void prvRecursiveMutexPollingTask( void *pvParameters ){	/* Just to remove compiler warning. */	( void ) pvParameters;	for( ;; )	{		/* Keep attempting to obtain the mutex.  We should only obtain it when		the blocking task has suspended itself, which in turn should only		happen when the controlling task is also suspended. */		if( xSemaphoreTakeRecursive( xMutex, recmuNO_DELAY ) == pdPASS )		{			/* Is the blocking task suspended? */			if( ( xBlockingIsSuspended != pdTRUE ) || ( xControllingIsSuspended != pdTRUE ) )			{				xErrorOccurred = pdTRUE;			}			else			{				/* Keep count of the number of cycles this task has performed 				so a stall can be detected. */				uxPollingCycles++;				/* We can resume the other tasks here even though they have a				higher priority than the polling task.  When they execute they				will attempt to obtain the mutex but fail because the polling				task is still the mutex holder.  The polling task (this task)				will then inherit the higher priority.  The Blocking task will				block indefinitely when it attempts to obtain the mutex, the				Controlling task will only block for a fixed period and an				error will be latched if the polling task has not returned the				mutex by the time this fixed period has expired. */				vTaskResume( xBlockingTaskHandle );                vTaskResume( xControllingTaskHandle );							/* The other two tasks should now have executed and no longer				be suspended. */				if( ( xBlockingIsSuspended == pdTRUE ) || ( xControllingIsSuspended == pdTRUE ) )				{					xErrorOccurred = pdTRUE;				}											/* Release the mutex, disinheriting the higher priority again. */				if( xSemaphoreGiveRecursive( xMutex ) != pdPASS )				{					xErrorOccurred = pdTRUE;				}			}		}		#if configUSE_PREEMPTION == 0		{			taskYIELD();		}		#endif	}}

/** * 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;}

/** * Execute the requested transaction on an object. * /param[in] connection UAVTalkConnection to be used * /param[in] type Transaction type *                        UAVTALK_TYPE_OBJ: send object, *                        UAVTALK_TYPE_OBJ_REQ: request object update *                        UAVTALK_TYPE_OBJ_ACK: send object with an ack * /param[in] obj Object * /param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. * /param[in] timeoutMs Time to wait for the ack, when zero it will return immediately * /return 0 Success * /return -1 Failure */static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs){    int32_t respReceived;    int32_t ret = -1;    // Send object depending on if a response is needed    if (type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS || type == UAVTALK_TYPE_OBJ_REQ) {        // Get transaction lock (will block if a transaction is pending)        xSemaphoreTakeRecursive(connection->transLock, portMAX_DELAY);        // Send object        xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);        // expected response type        connection->respType   = (type == UAVTALK_TYPE_OBJ_REQ) ? UAVTALK_TYPE_OBJ : UAVTALK_TYPE_ACK;        connection->respObjId  = UAVObjGetID(obj);        connection->respInstId = instId;        ret = sendObject(connection, type, UAVObjGetID(obj), instId, obj);        xSemaphoreGiveRecursive(connection->lock);        // Wait for response (or timeout) if sending the object succeeded        respReceived = pdFALSE;        if (ret == 0) {            respReceived = xSemaphoreTake(connection->respSema, timeoutMs / portTICK_RATE_MS);        }        // Check if a response was received        if (respReceived == pdTRUE) {            // We are done successfully            xSemaphoreGiveRecursive(connection->transLock);            ret = 0;        } else {            // Cancel transaction            xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);            // non blocking call to make sure the value is reset to zero (binary sema)            xSemaphoreTake(connection->respSema, 0);            connection->respObjId = 0;            xSemaphoreGiveRecursive(connection->lock);            xSemaphoreGiveRecursive(connection->transLock);            return -1;        }    } else if (type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_TS) {        xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);        ret = sendObject(connection, type, UAVObjGetID(obj), instId, obj);        xSemaphoreGiveRecursive(connection->lock);    }    return ret;}

/** * Update the status of all tasks */void TaskMonitorUpdateAll(void){#if defined(DIAGNOSTICS)	TaskInfoData data;	int n;	// Lock	xSemaphoreTakeRecursive(lock, portMAX_DELAY);#if ( configGENERATE_RUN_TIME_STATS == 1 )	uint32_t currentTime;	uint32_t deltaTime;		/*	 * Calculate the amount of elapsed run time between the last time we	 * measured and now. Scale so that we can convert task run times	 * directly to percentages.	 */	currentTime = portGET_RUN_TIME_COUNTER_VALUE();	deltaTime = ((currentTime - lastMonitorTime) / 100) ? : 1; /* avoid divide-by-zero if the interval is too small */	lastMonitorTime = currentTime;			#endif		// Update all task information	for (n = 0; n < TASKINFO_RUNNING_NUMELEM; ++n)	{		if (handles[n] != 0)		{			data.Running[n] = TASKINFO_RUNNING_TRUE;#if defined(ARCH_POSIX) || defined(ARCH_WIN32)			data.StackRemaining[n] = 10000;#else			data.StackRemaining[n] = uxTaskGetStackHighWaterMark(handles[n]) * 4;#if ( configGENERATE_RUN_TIME_STATS == 1 )			/* Generate run time stats */			data.RunningTime[n] = uxTaskGetRunTime(handles[n]) / deltaTime;			#endif#endif					}		else		{			data.Running[n] = TASKINFO_RUNNING_FALSE;			data.StackRemaining[n] = 0;			data.RunningTime[n] = 0;		}	}	// Update object	TaskInfoSet(&data);	// Done	xSemaphoreGiveRecursive(lock);#endif}

SimpleQueueValue* simpleQueueFront(struct SimpleQueue* queue){	while(xSemaphoreTakeRecursive(queue->xSemHandle,(TickType_t)0) != pdTRUE){}	SimpleQueueValue* return_Value = NULL;	if (simpleQueueEmpty(queue))		return_Value =  NULL;	else		return_Value =  queue->head->value;	xSemaphoreGiveRecursive(queue->xSemHandle);	 return return_Value;}

/*** @brief  Release a Recursive Mutex* @param   mutex_id      mutex ID obtained by /ref osRecursiveMutexCreate.* @retval  status code that indicates the execution status of the function.*/osStatus osRecursiveMutexRelease (osMutexId mutex_id){  osStatus result = osOK;    if (xSemaphoreGiveRecursive(mutex_id) != pdTRUE)   {    result = osErrorOS;  }  return result;}