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