xQueueSetMemberHandle MPU_xQueueSelectFromSet( xQueueSetHandle xQueueSet, portTickType xBlockTimeTicks )
{
xQueueSetMemberHandle xReturn;
portBASE_TYPE xRunningPrivileged = prvRaisePrivilege();
xReturn = xQueueSelectFromSet( xQueueSet, xBlockTimeTicks );
portRESET_PRIVILEGE( xRunningPrivileged );
return xReturn;
}
QueueSetMemberHandle_t MPU_xQueueSelectFromSet( QueueSetHandle_t xQueueSet, TickType_t xBlockTimeTicks )
{
QueueSetMemberHandle_t xReturn;
BaseType_t xRunningPrivileged = prvRaisePrivilege();
xReturn = xQueueSelectFromSet( xQueueSet, xBlockTimeTicks );
portRESET_PRIVILEGE( xRunningPrivileged );
return xReturn;
}
QueueSetMemberHandle_t MPU_xQueueSelectFromSet( QueueSetHandle_t xQueueSet, TickType_t xBlockTimeTicks )
{
QueueSetMemberHandle_t xReturn;
BaseType_t xRunningPrivileged = xPortRaisePrivilege();
xReturn = xQueueSelectFromSet( xQueueSet, xBlockTimeTicks );
vPortResetPrivilege( xRunningPrivileged );
return xReturn;
}
static void prvQueueSetReceivingTask( void *pvParameters )
{
uint32_t ulReceived;
QueueHandle_t xActivatedQueue;
TickType_t xBlockTime;
/* Remove compiler warnings. */
( void ) pvParameters;
/* Create the queues and add them to the queue set before resuming the Tx
task. */
prvSetupTest();
for( ;; ) {
/* For test coverage reasons, the block time is dependent on the
priority of this task - which changes during the test. When the task
is at the idle priority it polls the queue set. */
if( uxTaskPriorityGet( NULL ) == tskIDLE_PRIORITY ) {
xBlockTime = 0;
} else {
xBlockTime = portMAX_DELAY;
}
/* Wait for a message to arrive on one of the queues in the set. */
xActivatedQueue = xQueueSelectFromSet( xQueueSet, portMAX_DELAY );
if( xActivatedQueue == NULL ) {
if( xBlockTime != 0 ) {
/* This should not happen as an infinite delay was used. */
xQueueSetTasksStatus = pdFAIL;
}
} else {
/* Reading from the queue should pass with a zero block time as
this task will only run when something has been posted to a task
in the queue set. */
if( xQueueReceive( xActivatedQueue, &ulReceived, queuesetDONT_BLOCK ) != pdPASS ) {
xQueueSetTasksStatus = pdFAIL;
}
/* Ensure the value received was the value expected. This function
manipulates file scope data and is also called from an ISR, hence
the critical section. */
taskENTER_CRITICAL();
{
prvCheckReceivedValue( ulReceived );
}
taskEXIT_CRITICAL();
if( xQueueSetTasksStatus == pdPASS ) {
ulCycleCounter++;
}
}
}
}
void communication_task(){
int status;
rsc_info.rsc_tab = (struct resource_table *)&resources;
rsc_info.size = sizeof(resources);
zynqMP_r5_gic_initialize();
/* Initialize RPMSG framework */
status = remoteproc_resource_init(&rsc_info, rpmsg_channel_created, rpmsg_channel_deleted,
rpmsg_read_cb ,&proc);
if (status < 0) {
return;
}
#ifdef USE_FREERTOS
comm_queueset = xQueueCreateSet( 2 );
xQueueAddToSet( OpenAMPInstPtr.send_queue, comm_queueset);
xQueueAddToSet( OpenAMPInstPtr.lock, comm_queueset);
#else
env_create_sync_lock(&OpenAMPInstPtr.lock,LOCKED);
#endif
env_enable_interrupt(VRING1_IPI_INTR_VECT,0,0);
while (1) {
#ifdef USE_FREERTOS
QueueSetMemberHandle_t xActivatedMember;
xActivatedMember = xQueueSelectFromSet( comm_queueset, portMAX_DELAY);
if( xActivatedMember == OpenAMPInstPtr.lock ) {
env_acquire_sync_lock(OpenAMPInstPtr.lock);
process_communication(OpenAMPInstPtr);
}
if (xActivatedMember == OpenAMPInstPtr.send_queue) {
xQueueReceive( OpenAMPInstPtr.send_queue, &send_data, 0 );
rpmsg_send(app_rp_chnl, send_data.data, send_data.length);
}
#else
env_acquire_sync_lock(OpenAMPInstPtr.lock);
process_communication(OpenAMPInstPtr);
echo_test();
/* Wait for the result data on queue */
if(pq_qlength(OpenAMPInstPtr.send_queue) > 0) {
send_data = pq_dequeue(OpenAMPInstPtr.send_queue);
/* Send the result of echo_test back to master. */
rpmsg_send(app_rp_chnl, send_data->data, send_data->length);
}
#endif
}
}
static void prvQueueSetReceivingTask( void *pvParameters )
{
unsigned long ulReceived;
xQueueHandle xActivatedQueue;
xTaskHandle xQueueSetSendingTask;
/* The handle to the sending task is passed in using the task parameter. */
xQueueSetSendingTask = ( xTaskHandle ) pvParameters;
/* Create the queues and add them to the queue set before resuming the Tx
task. */
prvSetupTest( xQueueSetSendingTask );
for( ;; )
{
/* Wait for a message to arrive on one of the queues in the set. */
xActivatedQueue = xQueueSelectFromSet( xQueueSet, portMAX_DELAY );
configASSERT( xActivatedQueue );
if( xActivatedQueue == NULL )
{
/* This should not happen as an infinite delay was used. */
xQueueSetTasksStatus = pdFAIL;
}
else
{
/* Reading from the queue should pass with a zero block time as
this task will only run when something has been posted to a task
in the queue set. */
if( xQueueReceive( xActivatedQueue, &ulReceived, queuesetDONT_BLOCK ) != pdPASS )
{
xQueueSetTasksStatus = pdFAIL;
}
/* Ensure the value received was the value expected. This function
manipulates file scope data and is also called from an ISR, hence
the critical section. */
taskENTER_CRITICAL();
{
prvCheckReceivedValue( ulReceived );
}
taskEXIT_CRITICAL();
}
if( xQueueSetTasksStatus == pdPASS )
{
ulCycleCounter++;
}
}
}
static void btc_a2dp_sink_task_handler(void *arg)
{
QueueSetMemberHandle_t xActivatedMember;
BtTaskEvt_t *e = NULL;
for (;;) {
xActivatedMember = xQueueSelectFromSet(btc_aa_snk_queue_set, portMAX_DELAY);
if (xActivatedMember == btc_aa_snk_data_queue) {
int32_t data_evt;
xQueueReceive(xActivatedMember, &data_evt, 0);
if (data_evt == BTC_A2DP_SINK_DATA_EVT) {
btc_a2dp_sink_data_ready(NULL);
}
} else if (xActivatedMember == btc_aa_snk_ctrl_queue) {
xQueueReceive(xActivatedMember, &e, 0);
btc_a2dp_sink_ctrl_handler(e);
osi_free(e);
}
}
}
static void prvQueueSetReceivingTask( void *pvParameters )
{
uint32_t ulReceived, ulExpected = 0;
QueueHandle_t xActivatedQueue;
/* Remove compiler warnings. */
( void ) pvParameters;
for( ;; )
{
/* Is a message waiting? A block time is not used to ensure the queue
set is polled while it is being written to from an interrupt. */
xActivatedQueue = xQueueSelectFromSet( xQueueSet, setpollDONT_BLOCK );
if( xActivatedQueue != NULL )
{
/* Reading from the queue should pass with a zero block time as
this task will only run when something has been posted to a task
in the queue set. */
if( xQueueReceive( xActivatedQueue, &ulReceived, setpollDONT_BLOCK ) != pdPASS )
{
xQueueSetPollStatus = pdFAIL;
}
if( ulReceived == ulExpected )
{
ulExpected++;
}
else
{
xQueueSetPollStatus = pdFAIL;
}
if( xQueueSetPollStatus == pdPASS )
{
ulCycleCounter++;
}
}
}
}
static void prvSetupTest( void )
{
portBASE_TYPE x;
unsigned long ulValueToSend = 0;
/* Ensure the queues are created and the queue set configured before the
sending task is unsuspended.
First Create the queue set such that it will be able to hold a message for
every space in every queue in the set. */
xQueueSet = xQueueCreateSet( queuesetNUM_QUEUES_IN_SET * queuesetQUEUE_LENGTH );
for( x = 0; x < queuesetNUM_QUEUES_IN_SET; x++ )
{
/* Create the queue and add it to the set. The queue is just holding
unsigned long value. */
xQueues[ x ] = xQueueCreate( queuesetQUEUE_LENGTH, sizeof( unsigned long ) );
configASSERT( xQueues[ x ] );
if( xQueueAddToSet( xQueues[ x ], xQueueSet ) != pdPASS )
{
xQueueSetTasksStatus = pdFAIL;
}
else
{
/* The queue has now been added to the queue set and cannot be added to
another. */
if( xQueueAddToSet( xQueues[ x ], xQueueSet ) != pdFAIL )
{
xQueueSetTasksStatus = pdFAIL;
}
}
}
/* Attempt to remove a queue from a queue set it does not belong
to (NULL being passed as the queue set in this case). */
if( xQueueRemoveFromSet( xQueues[ 0 ], NULL ) != pdFAIL )
{
/* It is not possible to successfully remove a queue from a queue
set it does not belong to. */
xQueueSetTasksStatus = pdFAIL;
}
/* Attempt to remove a queue from the queue set it does belong to. */
if( xQueueRemoveFromSet( xQueues[ 0 ], xQueueSet ) != pdPASS )
{
/* It should be possible to remove the queue from the queue set it
does belong to. */
xQueueSetTasksStatus = pdFAIL;
}
/* Add an item to the queue before attempting to add it back into the
set. */
xQueueSend( xQueues[ 0 ], ( void * ) &ulValueToSend, 0 );
if( xQueueAddToSet( xQueues[ 0 ], xQueueSet ) != pdFAIL )
{
/* Should not be able to add a non-empty queue to a set. */
xQueueSetTasksStatus = pdFAIL;
}
/* Remove the item from the queue before adding the queue back into the
set so the dynamic tests can begin. */
xQueueReceive( xQueues[ 0 ], &ulValueToSend, 0 );
if( xQueueAddToSet( xQueues[ 0 ], xQueueSet ) != pdPASS )
{
/* If the queue was successfully removed from the queue set then it
should be possible to add it back in again. */
xQueueSetTasksStatus = pdFAIL;
}
/* The task that sends to the queues is not running yet, so attempting to
read from the queue set should fail. */
if( xQueueSelectFromSet( xQueueSet, queuesetSHORT_DELAY ) != NULL )
{
xQueueSetTasksStatus = pdFAIL;
}
/* Resume the task that writes to the queues. */
vTaskResume( xQueueSetSendingTask );
/* Let the ISR access the queues also. */
xSetupComplete = pdTRUE;
}
static void
main_thread(void *pdata) {
QueueSetHandle_t qs;
QueueSetMemberHandle_t active;
ASSERT((qs = xQueueCreateSet(SERIAL_RX_SIZE * 3)));
serial_start(cli_serial, 115200, qs);
serial_start(&Serial4, 57600, qs);
serial_start(&Serial5, cfg.gps_baud_rate ? cfg.gps_baud_rate : 57600, qs);
cli_set_output(cli_serial);
log_start(cli_serial);
cl_enabled = 1;
load_eeprom();
cfg.flags &= ~FLAG_HOLDOVER_TEST;
if (cfg.flags & FLAG_GPSEXT) {
gps_serial = &Serial5;
} else {
gps_serial = &Serial4;
}
if (!cfg.holdover) {
cfg.holdover = 60;
}
if (!cfg.loopstats_interval) {
cfg.loopstats_interval = 60;
}
ppscapture_start();
vtimer_start();
tcpip_start();
test_reset();
cli_banner();
if (!(cfg.flags & FLAG_GPSEXT)) {
ublox_configure();
if (HAS_FEATURE(PPSEN) && (cfg.flags & FLAG_PPSEN)) {
GPIO_OFF(PPSEN);
}
}
cl_enabled = 0;
while (1) {
watchdog_main = 5;
active = xQueueSelectFromSet(qs, pdMS_TO_TICKS(1000));
if (active == cli_serial->rx_q) {
int16_t val = serial_get(cli_serial, TIMEOUT_NOBLOCK);
ASSERT(val >= 0);
cli_feed(val);
} else if (active == gps_serial->rx_q) {
int16_t val = serial_get(gps_serial, TIMEOUT_NOBLOCK);
ASSERT(val >= 0);
gps_byte_received(val);
if (cfg.flags & FLAG_GPSOUT) {
char tmp = val;
serial_write(&Serial5, &tmp, 1);
}
#if 0
} else if (active == Serial5.rx_q) {
char tmp = serial_get(&Serial5, TIMEOUT_NOBLOCK);
serial_write(&Serial4, &tmp, 1);
#endif
}
}
}
void vTaskCommander (void *pvParameters)
{
QueueSetMemberHandle_t xActivatedMember;
RadioLinkData radioData;
IMUData imuData;
LogData log;
CommanderData commanderData;
static bool isArmed = false;
static float lastTick = xTaskGetTickCount();
//(const float kp, const float ki, const float kd, const float intLimit)
PidObject yawRatePid(0.3f, 0, 0.1f, 1);
PidObject pitchPid (2.0f, 10.0f, 0.13f, 10.0f);
PidObject rollPid (2.0f, 10.0f, 0.13f, 10.0f);
static bool imuReady = false; // We need at last one IMU data packet to start work.
static bool radioReady = false; // We need at last one radio data packet to start work.
while(1)
{
xActivatedMember = xQueueSelectFromSet(TheStabilizerQueueSet, 10);
if (xActivatedMember == TheRadioCommandsQueue)
{
xQueueReceive(TheRadioCommandsQueue, &radioData, 0 );
radioReady = true;
}
if (xActivatedMember == TheIMUDataQueue)
{
xQueueReceive(TheIMUDataQueue, &imuData, 0 );
imuReady = true;
}
else
{
// failure!
continue;
}
if (!imuReady || !radioReady)
{
// Wait for first complete data.
continue;
}
if (!isArmed)
{
// We can arm only if IMU and radio ready.
if (Arm(commanderData))
{
xQueueOverwrite( TheCommanderDataQueue, &commanderData );
continue;
}
// Arm done.
isArmed = true;
}
TheLedInfo->Y(true);
// dT calculation
const float currentTick = xTaskGetTickCount();
const float dT = (currentTick - lastTick) / 1000.0f; // Seconds
lastTick = currentTick;
if (dT == 0)
{
continue; // Skip first iteration.
}
if (radioData.Throttle <= 2 )
{
yawRatePid.Reset();
pitchPid.Reset();
rollPid.Reset();
commanderData.Throttle = 0;
commanderData.Pitch = 0;
commanderData.Roll = 0;
commanderData.Yaw = 0;
xQueueOverwrite( TheCommanderDataQueue, &commanderData );
continue;
}
// Convert radio to angles
const float angle_max = radians(30.0f);
const float yawRateDesired = map(radioData.Yaw, -100.0f, 100.0f, -angle_max, angle_max); // [-angle_max, angle_max] radians
const float pitchDesired = -map(radioData.Pitch, -100.0f, 100.0f, -angle_max, angle_max); // [-angle_max, angle_max] radians
const float rollDesired = map(radioData.Roll, -100.0f, 100.0f, -angle_max, angle_max); // [-angle_max, angle_max] radians
const float throttleDesired = map(radioData.Throttle, 0.0f, 100.0f, 10.0f, 100.0f); // [10, 100] percent of motor power
// Feed PID regulators
const float yawCorrection = yawRatePid.Update( yawRateDesired, imuData.Yaw, dT, true); // radians
const float pitchCorrection = pitchPid .Update( pitchDesired, imuData.Pitch, dT, true); // radians
const float rollCorrection = rollPid .Update( rollDesired, imuData.Roll, dT, true); // radians
// Calculate motors power
const float pm = 30.0f * throttleDesired / 100.0f; // Multiplier for Pitch
const float rm = 30.0f * throttleDesired / 100.0f; // Multiplier for Roll
const float ym = 5.0f; // Multiplier for Yaw
const float pitchK = pitchCorrection * pm; // [-1, 1] pitch correction
const float rollK = rollCorrection * rm; // [-1, 1] roll correction
const float yawK = cosf(yawCorrection); // [-1, 1] yaw correction
commanderData.Throttle = throttleDesired;
commanderData.Pitch = pitchK;
commanderData.Roll = rollK;
commanderData.Yaw = yawK;
xQueueOverwrite( TheCommanderDataQueue, &commanderData );
TheGlobalData.DBG_PID_YAW = yawCorrection;
TheGlobalData.DBG_PID_PITCH = pitchCorrection;
TheGlobalData.DBG_PID_ROLL = rollCorrection;
TheGlobalData.DBG_CO_YAW = yawK;
TheGlobalData.DBG_CO_PITCH = pitchK;
TheGlobalData.DBG_CO_ROLL = rollK;
TheLedInfo->Y(false);
/*
log.Timer = xTaskGetTickCount();
log.InputThrottle = radioData.Throttle;
log.InputYaw = radioData.Yaw;
log.InputPitch = radioData.Pitch;
log.InputRoll = radioData.Roll;
log.Yaw = imuData.EulierAngles.Z;
log.Roll = imuData.EulierAngles.Y;
log.Pitch = imuData.EulierAngles.X;
*/
// Testing telemetry.
//xQueueOverwrite( TheLogQueue, &log );
// Process Data!
}
vTaskDelete(NULL);
}
/**
* This is the C task that is created multiple times. Each task receives its
* own parameter therefore differentiating between different tasks.
*/
void scheduler_c_task_private(void *task_ptr)
{
scheduler_task& task = *(scheduler_task*)task_ptr;
/* Have the responsible task call taskEntry() for each task */
if (mTaskEntryTaskHandle == task.mHandle) {
bool failure = false;
print_strings(task.mName, " task calling taskEntry() for all tasks ... ");
for (uint32_t i=0; i < gTaskList->size(); i++)
{
scheduler_task *t = gTaskList->at(i);
if (!t->taskEntry()) {
print_strings(t->mName, " --> FAILED taskEntry()");
failure = true;
}
}
if (failure) {
puts("ERROR: Killing FreeRTOS due to error(s)");
vTaskEndScheduler();
} else {
/* Give permission for everyone to start the run() loop */
for (uint32_t i=0; i < gTaskList->size(); i++)
{
xSemaphoreGive(mRunTask);
}
}
}
// Wait until we're given the go ahead by the task giving semaphore above
xSemaphoreTake(mRunTask, portMAX_DELAY);
portTickType xLastWakeTime = xTaskGetTickCount();
portTickType xNextStatTime = 0;
for (;;)
{
#if (0 != configUSE_QUEUE_SETS)
if (task.mQueueSet) {
task.mQueueSetType = xQueueSelectFromSet(task.mQueueSet, task.mQueueSetBlockTime);
}
#endif
// Run the task code and suspend when an error occurs
if (!task.run((void*)task.mParam)) {
print_strings(task.mName, " --> FAILURE detected; suspending this task ...");
vTaskSuspend(0);
}
++(task.mRunCount);
// Update the task statistics once in a short while :
if (xTaskGetTickCount() > xNextStatTime) {
xNextStatTime = xTaskGetTickCount() + OS_MS(task.mStatUpdateRateMs);
task.mFreeStack = uxTaskGetStackHighWaterMark(task.mHandle);
task.mCpuPercent = uxTaskGetCpuUsage(task.mHandle);
}
// Delay if set
if (task.mTaskDelayMs) {
vTaskDelayUntil( &xLastWakeTime, OS_MS(task.mTaskDelayMs));
}
}
}
/**
* \brief Gatekeeper Task. Implementation of the gatekeeper task with his own loop.
* \param[in] pvParameters task parameters. Not used.
*/
void taskGatekeeper(void* pvParameters) {
QueueSetMemberHandle_t xActivatedMember;
message_t message;
char message_data[DATA_MESSAGE_STRING_LENGTH + 1] = { '\0' };
char *ptr;
char selector;
uint32_t i;
static const char frame_end[] = MSG_FRAME_END;
event_t event;
uint32_t timeout;
/* Loop forever */
for (;;) {
/* Get the message, which has to be sent */
xActivatedMember = xQueueSelectFromSet(queueMessageSet, portMAX_DELAY);
/* Check the type of the message */
if (xActivatedMember == queueMessageData) {
/* A data message */
xQueueReceive(queueMessageData, message_data, 0);
selector = MSG_TYPE_DATA;
ptr = message_data;
}
else if (xActivatedMember == queueMessage) {
/* A normal message */
xQueueReceive(queueMessage, &message, 0);
selector = message.type;
ptr = message.msg;
}
else {
/* Error event */
xActivatedMember = NULL;
}
if (xActivatedMember) {
/* Sets the timeout */
timeout = 20;
/* Takes the mutual exclusion to write into the circular buffer */
xSemaphoreTake(mutexTxCircBuf, portMAX_DELAY);
/* Send the message type selector */
while (!bsp_SerialCharPut(selector) && timeout > 0) {
/* No space available in the circular buffer */
vTaskDelay(10/portTICK_PERIOD_MS);
timeout--;
}
/* Send the string to the TX output buffer */
while (*ptr != '\0' && timeout > 0) {
while (!bsp_SerialCharPut(*ptr) && timeout > 0) {
/* No space available in the circular buffer */
vTaskDelay(10/portTICK_PERIOD_MS);
timeout--;
}
/* Next character */
ptr++;
}
/* Send the end of the message frame */
for (i=0; i<sizeof(frame_end)-1; i++) {
while (!bsp_SerialCharPut(frame_end[i]) && timeout > 0) {
/* No space available in the circular buffer */
vTaskDelay(10/portTICK_PERIOD_MS);
timeout--;
}
}
/* Check if there was a timeout */
if (timeout == 0) {
/* Sent the error event */
event.event = Marf_Serial;
xQueueSend(queueEvent, &event, portMAX_DELAY);
}
/* Release the mutual exclusion */
xSemaphoreGive(mutexTxCircBuf);
}
}
/* Never reach this point */
}
