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 ] ) );
}
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;
}
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++;
}
}
void MPU_vTaskSetApplicationTaskTag( TaskHandle_t xTask, TaskHookFunction_t pxTagValue )
{
BaseType_t xRunningPrivileged = prvRaisePrivilege();
vTaskSetApplicationTaskTag( xTask, pxTagValue );
portRESET_PRIVILEGE( xRunningPrivileged );
}
void MPU_vTaskSetApplicationTaskTag( TaskHandle_t xTask, TaskHookFunction_t pxTagValue )
{
BaseType_t xRunningPrivileged = xPortRaisePrivilege();
vTaskSetApplicationTaskTag( xTask, pxTagValue );
vPortResetPrivilege( xRunningPrivileged );
}
void MPU_vTaskSetApplicationTaskTag( xTaskHandle xTask, pdTASK_HOOK_CODE pxTagValue )
{
portBASE_TYPE xRunningPrivileged = prvRaisePrivilege();
vTaskSetApplicationTaskTag( xTask, pxTagValue );
portRESET_PRIVILEGE( xRunningPrivileged );
}
/**
* 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);
}
}
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 ] ) );
}
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(;;);
}
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;
}
}
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;
}
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();
}
}
}
}
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;
}
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
}
}
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();
}
}
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();
}
}
}
}
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
}
}
}
void BT_kSetThreadTag(void *pThreadID, void *pTagData) {
vTaskSetApplicationTaskTag((xTaskHandle) pThreadID, pTagData);
}
