/*********************************************************************
*
* _cbSendTaskList()
*
* Function description
* This function is part of the link between FreeRTOS and SYSVIEW.
* Called from SystemView when asked by the host, it uses SYSVIEW
* functions to send the entire task list to the host.
*/
static void _cbSendTaskList(void) {
TaskStatus_t* pxTaskStatusArray;
UBaseType_t uxArraySize;
UBaseType_t x;
char cStatus;
#if INCLUDE_xTaskGetIdleTaskHandle
TaskHandle_t hIdle;
hIdle = xTaskGetIdleTaskHandle();
#endif
/* Take a snapshot of the number of tasks in case it changes while this
function is executing. */
uxArraySize = uxTaskGetNumberOfTasks();
/* Allocate an array index for each task. */
pxTaskStatusArray = pvPortMalloc( uxArraySize * sizeof( TaskStatus_t ) );
if( pxTaskStatusArray != NULL ) {
/* Generate the (binary) data. */
uxArraySize = uxTaskGetSystemState( pxTaskStatusArray, uxArraySize, NULL );
#if INCLUDE_xTaskGetIdleTaskHandle
/* only get Idle task handle if scheduler has been started */
if (xTaskGetSchedulerState()!=taskSCHEDULER_NOT_STARTED) {
hIdle = xTaskGetIdleTaskHandle();
} else {
hIdle = NULL;
}
#endif
/* Create a human readable table from the binary data. */
for( x = 0; x < uxArraySize; x++ ) {
uint8_t* pStack;
#if INCLUDE_pxTaskGetStackStart
pStack = pxTaskGetStackStart(pxTaskStatusArray[x].xHandle);
#else
pStack = (uint8_t*)0;
#endif
#if INCLUDE_xTaskGetIdleTaskHandle
if (pxTaskStatusArray[x].xHandle != hIdle) {
SYSVIEW_SendTaskInfo((unsigned)pxTaskStatusArray[x].xHandle, pxTaskStatusArray[x].pcTaskName, pxTaskStatusArray[x].uxCurrentPriority, (unsigned int)pStack, 0);
}
#else
if (memcmp(pxTaskStatusArray[x].pcTaskName, "IDLE", 5) != 0) {
SYSVIEW_SendTaskInfo((unsigned)pxTaskStatusArray[x].xHandle, pxTaskStatusArray[x].pcTaskName, pxTaskStatusArray[x].uxCurrentPriority, (unsigned int)pStack, 0);
}
#endif
}
/* Free the array again. */
vPortFree( pxTaskStatusArray );
}
}
/**************************************************************************//**
* @brief vApplicationIdleHook
* Override the default definition of vApplicationIdleHook()
*****************************************************************************/
void vApplicationIdleHook(void)
{
#ifdef CHECK_STACK_USAGE
static bool b_tasks_registered = false;
if (!b_tasks_registered)
{
// Register Idle & Timer Task with stack check code
TaskHandle_t task;
task = xTaskGetIdleTaskHandle();
// Task init complete. Register task and some metrics with main info structure
task_info_t info;
info.task_handle = task;
info.stack_size = configMINIMAL_STACK_SIZE;
info.stack_use_percentage = 0;
register_task(TASK_IDLE, &info);
task = xTimerGetTimerDaemonTaskHandle();
info.task_handle = task;
info.stack_size = configTIMER_TASK_STACK_DEPTH;
info.stack_use_percentage = 0;
register_task(TASK_TIMER, &info);
b_tasks_registered = true;
}
#endif
}
xTaskHandle MPU_xTaskGetIdleTaskHandle( void )
{
xTaskHandle xReturn;
portBASE_TYPE xRunningPrivileged = prvRaisePrivilege();
xReturn = xTaskGetIdleTaskHandle();
portRESET_PRIVILEGE( xRunningPrivileged );
return eReturn;
}
TaskHandle_t MPU_xTaskGetIdleTaskHandle( void )
{
TaskHandle_t xReturn;
BaseType_t xRunningPrivileged = prvRaisePrivilege();
xReturn = xTaskGetIdleTaskHandle();
portRESET_PRIVILEGE( xRunningPrivileged );
return eReturn;
}
TaskHandle_t MPU_xTaskGetIdleTaskHandle( void )
{
TaskHandle_t xReturn;
BaseType_t xRunningPrivileged = xPortRaisePrivilege();
xReturn = xTaskGetIdleTaskHandle();
vPortResetPrivilege( xRunningPrivileged );
return xReturn;
}
void test_task_get_state(void* arg)
{
//Current task should return eRunning
TEST_ASSERT(eTaskGetState(xTaskGetCurrentTaskHandle()) == eRunning);
//Idle task of current core should return eReady
TEST_ASSERT(eTaskGetState(xTaskGetIdleTaskHandle()) == eReady);
//Blocked Task should return eBlocked
TEST_ASSERT(eTaskGetState(blocked_task_handle) == eBlocked);
//Suspended Task should return eSuspended
TEST_ASSERT(eTaskGetState(suspended_task_handle) == eSuspended);
xSemaphoreGive(done_sem);
vTaskDelete(NULL);
}
void print_task_list (emstream* ser_dev)
{
// Print the first line with the top of the headings
*ser_dev << PMS ("Task\t\t \t ")
#if (INCLUDE_uxTaskGetStackHighWaterMark == 1)
<< PMS ("\tStack")
#endif
<< endl;
// Print the second line with the rest of the headings
*ser_dev << PMS ("Name\t\tPri.\tState")
#if (INCLUDE_uxTaskGetStackHighWaterMark == 1)
<< PMS ("\tFree/Total")
#endif
<< PMS ("\tRuns") << endl;
// Print the third line which shows separators between headers and data
*ser_dev << PMS ("----\t\t----\t-----")
#if (INCLUDE_uxTaskGetStackHighWaterMark == 1)
<< PMS ("\t----------")
#endif
<< PMS ("\t----") << endl;
// Now have the tasks each print out their status. Tasks form a linked list, so
// we only need to get the last task started and it will call the next, etc.
if (last_created_task_pointer != NULL)
{
last_created_task_pointer->print_status_in_list (ser_dev);
}
// Have the idle task print out its information
*ser_dev << PMS ("IDLE\t\t0\t-\t")
#if (INCLUDE_uxTaskGetStackHighWaterMark == 1)
<< uxTaskGetStackHighWaterMark (xTaskGetIdleTaskHandle ())
<< PMS ("/") << configMINIMAL_STACK_SIZE << PMS ("\t\t")
#endif
#ifdef TASK_SETUP_AND_LOOP
<< PMS ("-")
#endif
<< endl;
}
void main_task(void *args)
{
vTaskSetApplicationTaskTag(xTaskGetIdleTaskHandle(), (pdTASK_HOOK_CODE)1);
vTaskSetApplicationTaskTag(NULL, (pdTASK_HOOK_CODE)2);
led_init();
hal.init(0, NULL);
hal.console->println("beginning stabilize app...");
//sensors_init();
userinput_init();
for(;;) {
debug_userinput();
vTaskDelay(100);
}
for(;;);
}
static void prvDemonstrateTaskStateAndHandleGetFunctions( void )
{
TaskHandle_t xIdleTaskHandle, xTimerTaskHandle;
char *pcTaskName;
static portBASE_TYPE xPerformedOneShotTests = pdFALSE;
TaskHandle_t xTestTask;
/* Demonstrate the use of the xTimerGetTimerDaemonTaskHandle() and
xTaskGetIdleTaskHandle() functions. Also try using the function that sets
the task number. */
xIdleTaskHandle = xTaskGetIdleTaskHandle();
xTimerTaskHandle = xTimerGetTimerDaemonTaskHandle();
/* This is the idle hook, so the current task handle should equal the
returned idle task handle. */
if( xTaskGetCurrentTaskHandle() != xIdleTaskHandle )
{
pcStatusMessage = "Error: Returned idle task handle was incorrect";
}
/* Check the timer task handle was returned correctly. */
pcTaskName = pcTaskGetTaskName( xTimerTaskHandle );
if( strcmp( pcTaskName, "Tmr Svc" ) != 0 )
{
pcStatusMessage = "Error: Returned timer task handle was incorrect";
}
/* This task is running, make sure it's state is returned as running. */
if( eTaskStateGet( xIdleTaskHandle ) != eRunning )
{
pcStatusMessage = "Error: Returned idle task state was incorrect";
}
/* If this task is running, then the timer task must be blocked. */
if( eTaskStateGet( xTimerTaskHandle ) != eBlocked )
{
pcStatusMessage = "Error: Returned timer task state was incorrect";
}
/* Other tests that should only be performed once follow. The test task
is not created on each iteration because to do so would cause the death
task to report an error (too many tasks running). */
if( xPerformedOneShotTests == pdFALSE )
{
/* Don't run this part of the test again. */
xPerformedOneShotTests = pdTRUE;
/* Create a test task to use to test other eTaskStateGet() return values. */
if( xTaskCreate( prvTestTask, "Test", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY, &xTestTask ) == pdPASS )
{
/* If this task is running, the test task must be in the ready state. */
if( eTaskStateGet( xTestTask ) != eReady )
{
pcStatusMessage = "Error: Returned test task state was incorrect 1";
}
/* Now suspend the test task and check its state is reported correctly. */
vTaskSuspend( xTestTask );
if( eTaskStateGet( xTestTask ) != eSuspended )
{
pcStatusMessage = "Error: Returned test task state was incorrect 2";
}
/* Now delete the task and check its state is reported correctly. */
vTaskDelete( xTestTask );
if( eTaskStateGet( xTestTask ) != eDeleted )
{
pcStatusMessage = "Error: Returned test task state was incorrect 3";
}
}
}
}
void main_task(void *args)
{
vTaskSetApplicationTaskTag(xTaskGetIdleTaskHandle(), (pdTASK_HOOK_CODE)1);
vTaskSetApplicationTaskTag(NULL, (pdTASK_HOOK_CODE)2);
led_init();
hal.init(0, NULL);
hal.console->printf("AP_HAL Sensor Test\r\n");
hal.scheduler->register_timer_failsafe(failsafe, 1000);
hal.console->printf("init AP_InertialSensor: ");
g_ins.init(AP_InertialSensor::COLD_START, INS_SAMPLE_RATE, flash_leds);
g_ins.init_accel(flash_leds);
hal.console->println();
led_set(0, false);
hal.console->printf("done\r\n");
hal.console->printf("init AP_Compass: "******"done\r\n");
hal.console->printf("init AP_Baro: ");
g_baro.init();
g_baro.calibrate();
hal.console->printf("done\r\n");
hal.console->printf("init AP_AHRS: ");
g_ahrs.init();
g_ahrs.set_compass(&g_compass);
g_ahrs.set_barometer(&g_baro);
hal.console->printf("done\r\n");
portTickType last_print = 0;
portTickType last_compass = 0;
portTickType last_wake = 0;
float heading = 0.0f;
last_wake = xTaskGetTickCount();
for (;;) {
// Delay to run this loop at 100Hz.
vTaskDelayUntil(&last_wake, 10);
portTickType now = xTaskGetTickCount();
if (last_compass == 0 || now - last_compass > 100) {
last_compass = now;
g_compass.read();
g_baro.read();
heading = g_compass.calculate_heading(g_ahrs.get_dcm_matrix());
}
g_ahrs.update();
if (last_print == 0 || now - last_print > 100) {
last_print = now;
hal.console->write("\r\n");
hal.console->printf("ahrs: roll %4.1f pitch %4.1f yaw %4.1f hdg %.1f\r\n",
ToDeg(g_ahrs.roll), ToDeg(g_ahrs.pitch),
ToDeg(g_ahrs.yaw),
g_compass.use_for_yaw() ? ToDeg(heading) : 0.0f);
Vector3f accel(g_ins.get_accel());
Vector3f gyro(g_ins.get_gyro());
hal.console->printf("mpu6000: accel %.2f %.2f %.2f "
"gyro %.2f %.2f %.2f\r\n",
accel.x, accel.y, accel.z,
gyro.x, gyro.y, gyro.z);
hal.console->printf("compass: heading %.2f deg\r\n",
ToDeg(g_compass.calculate_heading(0, 0)));
g_compass.null_offsets();
if (hal.rcin->valid()) {
uint16_t ppm[PPM_MAX_CHANNELS];
size_t count;
count = hal.rcin->read(ppm, PPM_MAX_CHANNELS);
hal.console->write("ppm: ");
for (size_t i = 0; i < count; ++i)
hal.console->printf("%u ", ppm[i]);
hal.console->write("\r\n");
}
}
}
}
STATIC mp_obj_t machine_info(uint n_args, const mp_obj_t *args) {
// FreeRTOS info
{
printf("---------------------------------------------\n");
printf("FreeRTOS\n");
printf("---------------------------------------------\n");
printf("Total heap: %u\n", configTOTAL_HEAP_SIZE);
printf("Free heap: %u\n", xPortGetFreeHeapSize());
printf("MpTask min free stack: %u\n", (unsigned int)uxTaskGetStackHighWaterMark((TaskHandle_t)mpTaskHandle));
printf("ServersTask min free stack: %u\n", (unsigned int)uxTaskGetStackHighWaterMark((TaskHandle_t)svTaskHandle));
printf("SlTask min free stack: %u\n", (unsigned int)uxTaskGetStackHighWaterMark(xSimpleLinkSpawnTaskHndl));
printf("IdleTask min free stack: %u\n", (unsigned int)uxTaskGetStackHighWaterMark(xTaskGetIdleTaskHandle()));
uint32_t *pstack = (uint32_t *)&_stack;
while (*pstack == 0x55555555) {
pstack++;
}
printf("MAIN min free stack: %u\n", pstack - ((uint32_t *)&_stack));
printf("---------------------------------------------\n");
}
return mp_const_none;
}
static void prvDemonstrateTaskStateAndHandleGetFunctions( void )
{
TaskHandle_t xIdleTaskHandle, xTimerTaskHandle;
char *pcTaskName;
static portBASE_TYPE xPerformedOneShotTests = pdFALSE;
TaskHandle_t xTestTask;
TaskStatus_t xTaskInfo;
extern StackType_t uxTimerTaskStack[];
/* Demonstrate the use of the xTimerGetTimerDaemonTaskHandle() and
xTaskGetIdleTaskHandle() functions. Also try using the function that sets
the task number. */
xIdleTaskHandle = xTaskGetIdleTaskHandle();
xTimerTaskHandle = xTimerGetTimerDaemonTaskHandle();
/* This is the idle hook, so the current task handle should equal the
returned idle task handle. */
if( xTaskGetCurrentTaskHandle() != xIdleTaskHandle )
{
pcStatusMessage = "Error: Returned idle task handle was incorrect";
}
/* Check the same handle is obtained using the idle task's name. First try
with the wrong name, then the right name. */
if( xTaskGetHandle( "Idle" ) == xIdleTaskHandle )
{
pcStatusMessage = "Error: Returned handle for name Idle was incorrect";
}
if( xTaskGetHandle( "IDLE" ) != xIdleTaskHandle )
{
pcStatusMessage = "Error: Returned handle for name Idle was incorrect";
}
/* Check the timer task handle was returned correctly. */
pcTaskName = pcTaskGetName( xTimerTaskHandle );
if( strcmp( pcTaskName, "Tmr Svc" ) != 0 )
{
pcStatusMessage = "Error: Returned timer task handle was incorrect";
}
if( xTaskGetHandle( "Tmr Svc" ) != xTimerTaskHandle )
{
pcStatusMessage = "Error: Returned handle for name Tmr Svc was incorrect";
}
/* This task is running, make sure it's state is returned as running. */
if( eTaskStateGet( xIdleTaskHandle ) != eRunning )
{
pcStatusMessage = "Error: Returned idle task state was incorrect";
}
/* If this task is running, then the timer task must be blocked. */
if( eTaskStateGet( xTimerTaskHandle ) != eBlocked )
{
pcStatusMessage = "Error: Returned timer task state was incorrect";
}
/* Also with the vTaskGetInfo() function. */
vTaskGetInfo( xTimerTaskHandle, /* The task being queried. */
&xTaskInfo, /* The structure into which information on the task will be written. */
pdTRUE, /* Include the task's high watermark in the structure. */
eInvalid ); /* Include the task state in the structure. */
/* Check the information returned by vTaskGetInfo() is as expected. */
if( ( xTaskInfo.eCurrentState != eBlocked ) ||
( strcmp( xTaskInfo.pcTaskName, "Tmr Svc" ) != 0 ) ||
( xTaskInfo.uxCurrentPriority != configTIMER_TASK_PRIORITY ) ||
( xTaskInfo.pxStackBase != uxTimerTaskStack ) ||
( xTaskInfo.xHandle != xTimerTaskHandle ) )
{
pcStatusMessage = "Error: vTaskGetInfo() returned incorrect information about the timer task";
}
/* Other tests that should only be performed once follow. The test task
is not created on each iteration because to do so would cause the death
task to report an error (too many tasks running). */
if( xPerformedOneShotTests == pdFALSE )
{
/* Don't run this part of the test again. */
xPerformedOneShotTests = pdTRUE;
/* Create a test task to use to test other eTaskStateGet() return values. */
if( xTaskCreate( prvTestTask, "Test", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY, &xTestTask ) == pdPASS )
{
/* If this task is running, the test task must be in the ready state. */
if( eTaskStateGet( xTestTask ) != eReady )
{
pcStatusMessage = "Error: Returned test task state was incorrect 1";
}
/* Now suspend the test task and check its state is reported correctly. */
vTaskSuspend( xTestTask );
if( eTaskStateGet( xTestTask ) != eSuspended )
{
pcStatusMessage = "Error: Returned test task state was incorrect 2";
}
/* Now delete the task and check its state is reported correctly. */
vTaskDelete( xTestTask );
if( eTaskStateGet( xTestTask ) != eDeleted )
{
pcStatusMessage = "Error: Returned test task state was incorrect 3";
}
}
}
}
uint8_t scheduler_task::getCpuIdlePercentage(void) const
{
return uxTaskGetCpuUsage(xTaskGetIdleTaskHandle());
}
