コード例 #1
0
void vStartFlopRegTests( void )
{
TaskHandle_t xTaskJustCreated;
unsigned portBASE_TYPE x, y, z = flopSTART_VALUE;

	/* Fill the arrays into which the flop registers are to be saved with 
	known values.  These are the values that will be written to the flop
	registers when the tasks start, and as the tasks do not perform any
	flop operations the values should never change.  Each position in the
	buffer contains a different value so the flop context of each task
	will be different. */
	for( x = 0; x < flopNUMBER_OF_TASKS; x++ )
	{
		for( y = 0; y < ( portNO_FLOP_REGISTERS_TO_SAVE - 1); y++ )
		{
			ulFlopRegisters[ x ][ y ] = z;
			z++;
		}
	}


	/* Create the first task. */
	xTaskCreate( vFlopTest1, "flop1", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY, &xTaskJustCreated );

	/* The task	tag value is a value that can be associated with a task, but 
	is not used by the scheduler itself.  Its use is down to the application so
	it makes a convenient place in this case to store the pointer to the buffer
	into which the flop context of the task will be stored.  The first created
	task uses ulFlopRegisters[ 0 ], the second ulFlopRegisters[ 1 ]. */
	vTaskSetApplicationTaskTag( xTaskJustCreated, ( void * ) &( ulFlopRegisters[ 0 ][ 0 ] ) );

	/* Do the same for the second task. */
	xTaskCreate( vFlopTest2, "flop2", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY, &xTaskJustCreated );
	vTaskSetApplicationTaskTag( xTaskJustCreated, ( void * ) &( ulFlopRegisters[ 1 ][ 0 ] ) );
}
コード例 #2
0
ファイル: Os.c プロジェクト: astaykov/ohNet
THandle OsThreadCreate(const char* aName, uint32_t aPriority, uint32_t aStackBytes, ThreadEntryPoint aEntryPoint, void* aArg)
{
    portBASE_TYPE result;
    xTaskHandle task;

    /* The task application tag is used to hold a pointer to the containing Thread object.
       If the child task is higher priority than it's parent, it'll run before the tag is 
       set. Bad. So we set the initial priority as slightly lower than parent thread, set
       the tag, then promote the child task to it's rightful priority. */

    result =  xTaskCreate(  aEntryPoint,                          // pdTASK_CODE pvTaskCode, 
                            (const signed char * const) aName,    // const portCHAR * const pcName, 
                            (aStackBytes != 0 ? aStackBytes : 1024 * 32) / sizeof( portSTACK_TYPE ),
                                                                  // unsigned portSHORT usStackDepth, 
                            aArg,                                 // void *pvParameters, 
                            uxTaskPriorityGet(NULL) - 1,          // unsigned portBASE_TYPE uxPriority, 
                            &task                                 // xTaskHandle *pvCreatedTask 
                          );

    if ( result != pdPASS )
        return kHandleNull;

    vTaskSetApplicationTaskTag(task, aArg);
    vTaskPrioritySet(task, aPriority);
    
    return (THandle) task;
}
コード例 #3
0
static void stabilizerTask(void* param)
{
  uint32_t tick = 0;
  uint32_t lastWakeTime;
  vTaskSetApplicationTaskTag(0, (void*)TASK_STABILIZER_ID_NBR);

  //Wait for the system to be fully started to start stabilization loop
  systemWaitStart();

  // Wait for sensors to be calibrated
  lastWakeTime = xTaskGetTickCount ();
  while(!sensorsAreCalibrated()) {
    vTaskDelayUntil(&lastWakeTime, F2T(RATE_MAIN_LOOP));
  }

  while(1) {
    vTaskDelayUntil(&lastWakeTime, F2T(RATE_MAIN_LOOP));

#ifdef ESTIMATOR_TYPE_kalman
    stateEstimatorUpdate(&state, &sensorData, &control);
#else
    sensorsAcquire(&sensorData, tick);
    stateEstimator(&state, &sensorData, tick);
#endif

    commanderGetSetpoint(&setpoint, &state);

    sitAwUpdateSetpoint(&setpoint, &sensorData, &state);

    stateController(&control, &sensorData, &state, &setpoint, tick);
    powerDistribution(&control);

    tick++;
  }
}
コード例 #4
0
ファイル: port.c プロジェクト: RitikaGupta1207/freertos
void MPU_vTaskSetApplicationTaskTag( TaskHandle_t xTask, TaskHookFunction_t pxTagValue )
{
    BaseType_t xRunningPrivileged = prvRaisePrivilege();

    vTaskSetApplicationTaskTag( xTask, pxTagValue );
    portRESET_PRIVILEGE( xRunningPrivileged );
}
コード例 #5
0
ファイル: mpu_wrappers.c プロジェクト: sean93park/freertos
	void MPU_vTaskSetApplicationTaskTag( TaskHandle_t xTask, TaskHookFunction_t pxTagValue )
	{
	BaseType_t xRunningPrivileged = xPortRaisePrivilege();

		vTaskSetApplicationTaskTag( xTask, pxTagValue );
		vPortResetPrivilege( xRunningPrivileged );
	}
コード例 #6
0
ファイル: port.c プロジェクト: ADTL/ARMWork
	void MPU_vTaskSetApplicationTaskTag( xTaskHandle xTask, pdTASK_HOOK_CODE pxTagValue )
	{
    portBASE_TYPE xRunningPrivileged = prvRaisePrivilege();

		vTaskSetApplicationTaskTag( xTask, pxTagValue );
        portRESET_PRIVILEGE( xRunningPrivileged );
	}
コード例 #7
0
ファイル: proximity.c プロジェクト: FreeRTOSHAL/copterOS
/**
 * Proximity task running at PROXIMITY_TASK_FREQ Hz.
 *
 * @param param Currently unused.
 */
static void proximityTask(void* param)
{
  uint32_t lastWakeTime;

  vTaskSetApplicationTaskTag(0, (void*)TASK_PROXIMITY_ID_NBR);

  //Wait for the system to be fully started to start stabilization loop
  systemWaitStart();

  lastWakeTime = xTaskGetTickCount();

  while(1)
  {
    vTaskDelayUntil(&lastWakeTime, F2T(PROXIMITY_TASK_FREQ));

#if defined(MAXSONAR_ENABLED)
    /* Read the MaxBotix sensor. */
    proximityDistance = maxSonarReadDistance(MAXSONAR_MB1040_AN, &proximityAccuracy);
#endif

    /* Get the latest average value calculated. */
    proximityDistanceAvg = proximitySWinAdd(proximityDistance);

    /* Get the latest median value calculated. */
    proximityDistanceMedian = proximitySWinMedian(proximitySWin);
  }
}
コード例 #8
0
ファイル: flop.c プロジェクト: BuiChien/FreeRTOS-TM4C123GXL
void vStartMathTasks( unsigned portBASE_TYPE uxPriority )
{
TaskHandle_t xTaskJustCreated;
portBASE_TYPE x, y;

	/* Place known values into the buffers into which the flop registers are 
	to be saved.  This is for debug purposes only, it is not normally
	required.  The last position in each array is left at zero as the status
	register will be loaded from there. 
	
	It is intended that these values can be viewed being loaded into the
	flop registers when a task is started - however the Insight debugger
	does not seem to want to show the flop register values. */
	for( x = 0; x < mathNUMBER_OF_TASKS; x++ )
	{
		for( y = 0; y < ( portNO_FLOP_REGISTERS_TO_SAVE - 1 ); y++ )
		{
			ulFlopRegisters[ x ][ y ] = ( x + 1 );
		}
	}

	/* Create the first task - passing it the address of the check variable
	that it is going to increment.  This check variable is used as an 
	indication that the task is still running. */
	xTaskCreate( vCompetingMathTask1, "Math1", mathSTACK_SIZE, ( void * ) &( usTaskCheck[ 0 ] ), uxPriority, &xTaskJustCreated );

	/* The task	tag value is a value that can be associated with a task, but 
	is not used by the scheduler itself.  Its use is down to the application so
	it makes a convenient place in this case to store the pointer to the buffer
	into which the flop context of the task will be stored.  The first created
	task uses ulFlopRegisters[ 0 ], the second ulFlopRegisters[ 1 ], etc. */
	vTaskSetApplicationTaskTag( xTaskJustCreated, ( void * ) &( ulFlopRegisters[ 0 ][ 0 ] ) );

	/* Create another 7 tasks, allocating a buffer for each. */
	xTaskCreate( vCompetingMathTask2, "Math2", mathSTACK_SIZE, ( void * ) &( usTaskCheck[ 1 ] ), uxPriority, &xTaskJustCreated  );
	vTaskSetApplicationTaskTag( xTaskJustCreated, ( void * ) &( ulFlopRegisters[ 1 ][ 0 ] ) );

	xTaskCreate( vCompetingMathTask3, "Math3", mathSTACK_SIZE, ( void * ) &( usTaskCheck[ 2 ] ), uxPriority, &xTaskJustCreated  );
	vTaskSetApplicationTaskTag( xTaskJustCreated, ( void * ) &( ulFlopRegisters[ 2 ][ 0 ] ) );

	xTaskCreate( vCompetingMathTask4, "Math4", mathSTACK_SIZE, ( void * ) &( usTaskCheck[ 3 ] ), uxPriority, &xTaskJustCreated  );
	vTaskSetApplicationTaskTag( xTaskJustCreated, ( void * ) &( ulFlopRegisters[ 3 ][ 0 ] ) );

	xTaskCreate( vCompetingMathTask1, "Math5", mathSTACK_SIZE, ( void * ) &( usTaskCheck[ 4 ] ), uxPriority, &xTaskJustCreated  );
	vTaskSetApplicationTaskTag( xTaskJustCreated, ( void * ) &( ulFlopRegisters[ 4 ][ 0 ] ) );

	xTaskCreate( vCompetingMathTask2, "Math6", mathSTACK_SIZE, ( void * ) &( usTaskCheck[ 5 ] ), uxPriority, &xTaskJustCreated  );
	vTaskSetApplicationTaskTag( xTaskJustCreated, ( void * ) &( ulFlopRegisters[ 5 ][ 0 ] ) );

	xTaskCreate( vCompetingMathTask3, "Math7", mathSTACK_SIZE, ( void * ) &( usTaskCheck[ 6 ] ), uxPriority, &xTaskJustCreated  );
	vTaskSetApplicationTaskTag( xTaskJustCreated, ( void * ) &( ulFlopRegisters[ 6 ][ 0 ] ) );

	xTaskCreate( vCompetingMathTask4, "Math8", mathSTACK_SIZE, ( void * ) &( usTaskCheck[ 7 ] ), uxPriority, &xTaskJustCreated  );
	vTaskSetApplicationTaskTag( xTaskJustCreated, ( void * ) &( ulFlopRegisters[ 7 ][ 0 ] ) );
}
コード例 #9
0
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(;;);
}
コード例 #10
0
ファイル: rtos_pilot.c プロジェクト: Safieddine90/gluonpilot
void vApplicationIdleHook( void )
{
    vTaskSetApplicationTaskTag( NULL, ( void * ) 8 );
    TRISBbits.TRISB2 = 0;  // use OSD SPI CS
    while (1)
    {
        PORTBbits.RB2 = 0;
        idle_counter+=2;
        PORTBbits.RB2 = 1;
        idle_counter-=1;
    }
}
コード例 #11
0
void wifiInit()
{
  if(isInit)
    return;

  esp8266Init();

  vTaskSetApplicationTaskTag(0, (void *)TASK_RADIO_ID_NBR);

    /* Launch the Radio link task */
  xTaskCreate(wifilinkTask, (const char * )"WiFiLink",
              configMINIMAL_STACK_SIZE, NULL, /*priority*/4, NULL);

  isInit = true;
}
コード例 #12
0
ファイル: stabilizer.c プロジェクト: Solomute/soloflie
static void stabilizerTask(void* param)
{
  uint32_t lastWakeTime;
  vTaskSetApplicationTaskTag(0, (void*)TASK_STABILIZER_ID_NBR);

  //Wait for the system to be fully started to start stabilization loop
  systemWaitStart();

  lastWakeTime = xTaskGetTickCount ();

  while(1)
  {
    vTaskDelayUntil(&lastWakeTime, F2T(IMU_UPDATE_FREQ));

    // Magnetometer not yet used more then for logging.
    imu9Read(&gyro, &acc, &mag);

    if (imu6IsCalibrated())
    {
      commanderGetRPY(&rollDesired, &pitchDesired, &yawDesired);

      sensfusion6UpdateQ(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z, FUSION_UPDATE_DT);
      sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual);
       
      if (horizonMode) {
        horizonPID(eulerRollActual, eulerPitchActual, -gyro.z,
        rollDesired, pitchDesired, yawDesired);
      } else {
        ratePID(gyro.x, -gyro.y, -gyro.z,
        rollDesired, pitchDesired, yawDesired);
      }

      controllerGetActuatorOutput(&actuatorRoll, &actuatorPitch, &actuatorYaw);

      commanderGetThrust(&actuatorThrust);
    
      /* Call out before performing thrust updates, if any functions would like to influence the thrust. */
     if (armed) {
      distributePower(actuatorThrust, actuatorRoll, actuatorPitch, actuatorYaw);
    
  } else {
    distributePower(0, 0, 0, 0);
    controllerResetAllPID();
  }
}
  }
}
コード例 #13
0
ファイル: port.c プロジェクト: Carrotman42/cs4534
portBASE_TYPE xPortUsesFloatingPoint( xTaskHandle xTask )
{
unsigned long *pulFlopBuffer;
portBASE_TYPE xReturn;
extern void * volatile pxCurrentTCB;

	/* This function tells the kernel that the task referenced by xTask is
	going to use the floating point registers and therefore requires the
	floating point registers saved as part of its context. */

	/* Passing NULL as xTask is used to indicate that the calling task is the
	subject task - so pxCurrentTCB is the task handle. */
	if( xTask == NULL )
	{
		xTask = ( xTaskHandle ) pxCurrentTCB;
	}

	/* Allocate a buffer large enough to hold all the flop registers. */
	pulFlopBuffer = ( unsigned long * ) pvPortMalloc( portFLOP_STORAGE_SIZE );
	
	if( pulFlopBuffer != NULL )
	{
		/* Start with the registers in a benign state. */
		memset( ( void * ) pulFlopBuffer, 0x00, portFLOP_STORAGE_SIZE );
		
		/* The first thing to get saved in the buffer is the FPSCR value -
		initialise this to the current FPSCR value. */
		*pulFlopBuffer = get_fpscr();
		
		/* Use the task tag to point to the flop buffer.  Pass pointer to just 
		above the buffer because the flop save routine uses a pre-decrement. */
		vTaskSetApplicationTaskTag( xTask, ( void * ) ( pulFlopBuffer + portFLOP_REGISTERS_TO_STORE ) );		
		xReturn = pdPASS;
	}
	else
	{
		xReturn = pdFAIL;
	}
	
	return xReturn;
}
コード例 #14
0
ファイル: adc.c プロジェクト: nongxiaoming/MiniQuadcopter
void adcTask(void *param)
{
  AdcGroup* adcRawValues;
  AdcGroup adcValues;

  vTaskSetApplicationTaskTag(0, (void*)TASK_ADC_ID_NBR);
  vTaskDelay(1000);

  adcDmaStart();

  while(1)
  {
    xQueueReceive(adcQueue, &adcRawValues, portMAX_DELAY);
    adcDecimate(adcRawValues, &adcValues);  // 10% CPU
    pmBatteryUpdate(&adcValues);

#ifdef ADC_OUTPUT_RAW_DATA
    uartSendDataDma(sizeof(AdcGroup)*ADC_MEAN_SIZE, (uint8_t*)adcRawValues);
#endif
  }
}
コード例 #15
0
static void vErrorChecks( void *pvParameters )
{
portTickType xExpectedWakeTime;
const portTickType xPrintRate = ( portTickType ) 5000 / portTICK_RATE_MS;
const long lMaxAllowableTimeDifference = ( long ) 0;
portTickType xWakeTime;
long lTimeDifference;
const char *pcReceivedMessage;
const char * const pcTaskBlockedTooLongMsg = "Print task blocked too long!\r\n";
const char * const pcUnexpectedHookValueMsg = "Task hook has unexpected value!\r\n";

	( void ) pvParameters;

	/* Register our callback function. */
	vTaskSetApplicationTaskTag( NULL, prvExampleTaskHook );
	
	/* Just for test purposes. */
	if( xTaskGetApplicationTaskTag( NULL ) != prvExampleTaskHook )
	{
		vPrintDisplayMessage( &pcUnexpectedHookValueMsg );
	}

	/* Loop continuously, blocking, then checking all the other tasks are still
	running, before blocking once again.  This task blocks on the queue of
	messages that require displaying so will wake either by its time out expiring,
	or a message becoming available. */
	for( ;; )
	{
		/* Calculate the time we will unblock if no messages are received
		on the queue.  This is used to check that we have not blocked for too long. */
		xExpectedWakeTime = xTaskGetTickCount();
		xExpectedWakeTime += xPrintRate;

		/* Block waiting for either a time out or a message to be posted that
		required displaying. */
		pcReceivedMessage = pcPrintGetNextMessage( xPrintRate );

		/* Was a message received? */
		if( pcReceivedMessage == NULL )
		{
			/* A message was not received so we timed out, did we unblock at the
			expected time? */
			xWakeTime = xTaskGetTickCount();

			/* Calculate the difference between the time we unblocked and the
			time we should have unblocked. */
			if( xWakeTime > xExpectedWakeTime )
			{
				lTimeDifference = ( long ) ( xWakeTime - xExpectedWakeTime );
			}
			else
			{
				lTimeDifference = ( long ) ( xExpectedWakeTime - xWakeTime );
			}

			if( lTimeDifference > lMaxAllowableTimeDifference )
			{
				/* We blocked too long - create a message that will get
				printed out the next time around.  If we are not using
				preemption then we won't expect the timing to be so
				accurate. */
				if( sUsingPreemption == pdTRUE )
				{
					vPrintDisplayMessage( &pcTaskBlockedTooLongMsg );
				}
			}

			/* Check the other tasks are still running, just in case. */
			prvCheckOtherTasksAreStillRunning();
		}
		else
		{
			/* We unblocked due to a message becoming available.  Send the message
			for printing. */
			vDisplayMessage( pcReceivedMessage );
		}

		/* Key presses are used to invoke the trace visualisation utility, or end
		the program. */
		prvCheckForKeyPresses();
	}
}
コード例 #16
0
static void stabilizerTask(void* param) {
	uint32_t attitudeCounter = 0;
	uint32_t altHoldCounter = 0;
	uint32_t lastWakeTime;

	vTaskSetApplicationTaskTag(0, (void*) TASK_STABILIZER_ID_NBR);

	//Wait for the system to be fully started to start stabilization loop
	systemWaitStart();

	lastWakeTime = xTaskGetTickCount();

	while (1) {
		vTaskDelayUntil(&lastWakeTime, F2T(IMU_UPDATE_FREQ)); // 500Hz

		// Magnetometer not yet used more then for logging.
		imu9Read(&gyro, &acc, &mag);

		if (imu6IsCalibrated()) {
			commanderGetRPY(&eulerRollDesired, &eulerPitchDesired, &eulerYawDesired);
			commanderGetRPYType(&rollType, &pitchType, &yawType);

			// 250HZ
			if (++attitudeCounter >= ATTITUDE_UPDATE_RATE_DIVIDER) {
				sensfusion6UpdateQ(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z, FUSION_UPDATE_DT);
				sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual);

				accWZ = sensfusion6GetAccZWithoutGravity(acc.x, acc.y, acc.z);
				accMAG = (acc.x * acc.x) + (acc.y * acc.y) + (acc.z * acc.z);
				// Estimate speed from acc (drifts)
				vSpeed += deadband(accWZ, vAccDeadband) * FUSION_UPDATE_DT;

				controllerCorrectAttitudePID(eulerRollActual, eulerPitchActual, eulerYawActual,
						eulerRollDesired, eulerPitchDesired, -eulerYawDesired, &rollRateDesired,
						&pitchRateDesired, &yawRateDesired);
				attitudeCounter = 0;
			}

			// 100HZ
			if (imuHasBarometer() && (++altHoldCounter >= ALTHOLD_UPDATE_RATE_DIVIDER)) {
				stabilizerAltHoldUpdate();
				altHoldCounter = 0;
			}

			if (rollType == RATE) {
				rollRateDesired = eulerRollDesired;
			}
			if (pitchType == RATE) {
				pitchRateDesired = eulerPitchDesired;
			}
			if (yawType == RATE) {
				yawRateDesired = -eulerYawDesired;
			}

			// TODO: Investigate possibility to subtract gyro drift.
			controllerCorrectRatePID(gyro.x, -gyro.y, gyro.z, rollRateDesired, pitchRateDesired,
					yawRateDesired);

			controllerGetActuatorOutput(&actuatorRoll, &actuatorPitch, &actuatorYaw);

			if (!altHold || !imuHasBarometer()) {
				// Use thrust from controller if not in altitude hold mode
				commanderGetThrust(&actuatorThrust);
			} else {
				// Added so thrust can be set to 0 while in altitude hold mode after disconnect
				commanderWatchdog();
			}

			if (actuatorThrust > 0) {
#if defined(TUNE_ROLL)
				distributePower(actuatorThrust, actuatorRoll, 0, 0);
#elif defined(TUNE_PITCH)
				distributePower(actuatorThrust, 0, actuatorPitch, 0);
#elif defined(TUNE_YAW)
				distributePower(actuatorThrust, 0, 0, -actuatorYaw);
#else
				distributePower(actuatorThrust, actuatorRoll, actuatorPitch, -actuatorYaw);
#endif
			} else {
				distributePower(0, 0, 0, 0);
				controllerResetAllPID();
			}
		}
	}
}
コード例 #17
0
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 void stabilizerTask(void* param)
{
  uint32_t attitudeCounter = 0;
  uint32_t lastWakeTime;

  vTaskSetApplicationTaskTag(0, (void*)TASK_STABILIZER_ID_NBR);

  //Wait for the system to be fully started to start stabilization loop
  systemWaitStart();

  lastWakeTime = xTaskGetTickCount ();

  while(1)
  {
    vTaskDelayUntil(&lastWakeTime, F2T(IMU_UPDATE_FREQ));

    imu6Read(&gyro, &acc);

    hmc5883lGetHeading(&magHeadingX, &magHeadingY, &magHeadingZ);

    if(altMode == ALTIMETER_MODE_PRESSURE) {
        altimeterTmp = ms5611GetPressure(MS5611_OSR_4096);
        if(altimeterTmp > 0.0f) {
            pressure = altimeterTmp;
            altMode = ALTIMETER_MODE_TEMPERATURE;
        }
    }

    if(altMode == ALTIMETER_MODE_TEMPERATURE) {
    	altimeterTmp = ms5611GetTemperature(MS5611_OSR_4096);
    	if(altimeterTmp > 0.0f) {
    		temperature = altimeterTmp;
    		altMode = ALTIMETER_MODE_PRESSURE;
    	}
    }

    if (imu6IsCalibrated())
    {
      commanderGetRPY(&eulerRollDesired, &eulerPitchDesired, &eulerYawDesired);
      commanderGetRPYType(&rollType, &pitchType, &yawType);

      if (++attitudeCounter >= ATTITUDE_UPDATE_RATE_DIVIDER)
      {
        //sensfusion6UpdateQ(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z, FUSION_UPDATE_DT);
        //sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual);

        //controllerCorrectAttitudePID(eulerRollActual, eulerPitchActual, eulerYawActual,
        //                             eulerRollDesired, eulerPitchDesired, -eulerYawDesired,
        //                             &rollRateDesired, &pitchRateDesired, &yawRateDesired);
    	//controllerCorrectAttitudePID(0, 0, 0, 0, 0, 0, &rollRateDesired, &pitchRateDesired, &yawRateDesired);
        attitudeCounter = 0;
      }

      if (rollType == RATE)
      {
        rollRateDesired = eulerRollDesired;
      }
      if (pitchType == RATE)
      {
        pitchRateDesired = eulerPitchDesired;
      }
      if (yawType == RATE)
      {
        yawRateDesired = -eulerYawDesired;
      }

      	  /*
      // TODO: Investigate possibility to subtract gyro drift.
      controllerCorrectRatePID(gyro.x, -gyro.y, gyro.z,
                               rollRateDesired, pitchRateDesired, yawRateDesired);

      controllerGetActuatorOutput(&actuatorRoll, &actuatorPitch, &actuatorYaw);
	*/
      commanderGetTrust(&actuatorThrust);
      distributePower(actuatorThrust, 0, 0, 0);

      if (actuatorThrust > 0)
      {
#if defined(TUNE_ROLL)
        distributePower(actuatorThrust, actuatorRoll, 0, 0);
#elif defined(TUNE_PITCH)
        distributePower(actuatorThrust, 0, actuatorPitch, 0);
#elif defined(TUNE_YAW)
        distributePower(actuatorThrust, 0, 0, -actuatorYaw);
#else
        distributePower(actuatorThrust, actuatorRoll, actuatorPitch, -actuatorYaw);
#endif
      }
      else
      {
        distributePower(0, 0, 0, 0);
        controllerResetAllPID();
      }
#if 0
     static int i = 0;
      if (++i > 19)
      {
        uartPrintf("%i, %i, %i\n",
            (int32_t)(eulerRollActual*100),
            (int32_t)(eulerPitchActual*100),
            (int32_t)(eulerYawActual*100));
        i = 0;
      }
#endif
    }
  }
}
コード例 #19
0
void BT_kSetThreadTag(void *pThreadID, void *pTagData) {
	vTaskSetApplicationTaskTag((xTaskHandle) pThreadID, pTagData);
}