rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-30 08:24:11 +01:00
Code Issues Releases Activity

Bring back the original module files and remove comments from FreeRTOS

Browse Source 
debugging
This commit is contained in:
James Cotton 2012-03-14 16:52:52 -05:00
parent 82a2987b2c
commit 6df9691609
5 changed files with 2 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
flight/Modules/Attitude/revolution/attitude.c
View File

@ -209,15 +209,6 @@ static void AttitudeTask(void *parameters)
first_run = true;
}
static int i;
static uint32_t last_time;
i++;
if (i % 5000 == 0) {
float dT = PIOS_DELAY_DiffuS(last_time) / 10.0e6;
fprintf(stderr, "Attitude relative timing: %f\n", dT);
last_time = PIOS_DELAY_GetRaw();
}
// This function blocks on data queue
switch (revoSettings.FusionAlgorithm ) {
case REVOSETTINGS_FUSIONALGORITHM_COMPLIMENTARY:

32
flight/Modules/Stabilization/stabilization.c
View File

@ -138,8 +138,6 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart)
/**
* Module task
*/
int stabilization_step;
int stabilization_count;
static void stabilizationTask(void* parameters)
{
UAVObjEvent ev;
@ -160,19 +158,6 @@ static void stabilizationTask(void* parameters)
while(1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
stabilization_step = 0;
stabilization_count ++;
static int i;
static uint32_t last_time;
i++;
if (i % 5000 == 0) {
float dT = PIOS_DELAY_DiffuS(last_time) / 10.0e6;
fprintf(stderr, "Stabilization relative timing: %f\n", dT);
last_time = PIOS_DELAY_GetRaw();
}
stabilization_step = 1;
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
{
@ -180,8 +165,6 @@ static void stabilizationTask(void* parameters)
continue;
}
stabilization_step = 2;
dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f;
timeval = PIOS_DELAY_GetRaw();
@ -189,8 +172,6 @@ static void stabilizationTask(void* parameters)
StabilizationDesiredGet(&stabDesired);
AttitudeActualGet(&attitudeActual);
GyrosGet(&gyrosData);
stabilization_step = 3;
#if defined(DIAGNOSTICS)
RateDesiredGet(&rateDesired);
@ -203,8 +184,6 @@ static void stabilizationTask(void* parameters)
float q_error[4];
float local_error[3];
stabilization_step = 4;
// Essentially zero errors for anything in rate or none
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
rpy_desired[0] = stabDesired.Roll;
@ -227,8 +206,6 @@ static void stabilizationTask(void* parameters)
quat_inverse(q_error);
Quaternion2RPY(q_error, local_error);
stabilization_step = 5;
#else
// Simpler algorithm for CC, less memory
float local_error[3] = {stabDesired.Roll - attitudeActual.Roll,
@ -245,8 +222,6 @@ static void stabilizationTask(void* parameters)
float *attitudeDesiredAxis = &stabDesired.Roll;
float *actuatorDesiredAxis = &actuatorDesired.Roll;
float *rateDesiredAxis = &rateDesired.Roll;
stabilization_step = 6;
//Calculate desired rate
for(uint8_t i=0; i< MAX_AXES; i++)
@ -314,8 +289,6 @@ static void stabilizationTask(void* parameters)
}
}
stabilization_step = 7;
uint8_t shouldUpdate = 1;
#if defined(DIAGNOSTICS)
RateDesiredSet(&rateDesired);
@ -362,8 +335,6 @@ static void stabilizationTask(void* parameters)
}
}
stabilization_step = 8;
// Save dT
actuatorDesired.UpdateTime = dT * 1000;
@ -378,8 +349,6 @@ static void stabilizationTask(void* parameters)
ActuatorDesiredSet(&actuatorDesired);
}
stabilization_step = 9;
if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
(lowThrottleZeroIntegral && stabDesired.Throttle < 0) ||
!shouldUpdate)
@ -387,7 +356,6 @@ static void stabilizationTask(void* parameters)
ZeroPids();
}
stabilization_step = 10;
// Clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);

4
flight/Modules/System/systemmod.c
View File

@ -50,7 +50,7 @@
#include "taskmonitor.h"
// Private constants
#define SYSTEM_UPDATE_PERIOD_MS 5000
#define SYSTEM_UPDATE_PERIOD_MS 1000
#define LED_BLINK_RATE_HZ 5
#ifndef IDLE_COUNTS_PER_SEC_AT_NO_LOAD
@ -197,7 +197,7 @@ static void systemTask(void *parameters)
if(flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) );
} else {
vTaskDelayUntil(&lastSysTime, 50); //SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS);
vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS);
}
}
}

4
flight/Modules/Telemetry/telemetry.c
View File

@ -274,7 +274,6 @@ static void telemetryTxTask(void *parameters)
while (1) {
// Wait for queue message
if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) {
//fprintf(stderr,".");
// Process event
processObjEvent(&ev);
}
@ -293,8 +292,6 @@ static void telemetryTxPriTask(void *parameters)
while (1) {
// Wait for queue message
if (xQueueReceive(priorityQueue, &ev, portMAX_DELAY) == pdTRUE) {
//fprintf(stderr,"*");
// Process event
processObjEvent(&ev);
}
@ -311,7 +308,6 @@ static void telemetryRxTask(void *parameters)
// Task loop
while (1) {
//fprintf(stderr,"-");
#if defined(PIOS_INCLUDE_USB)
// Determine input port (USB takes priority over telemetry port)
if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB) {

1
flight/UAVObjects/eventdispatcher.c
View File

@ -288,7 +288,6 @@ static void eventTask()
{
// Calculate delay time
delayMs = timeToNextUpdateMs-(xTaskGetTickCount()*portTICK_RATE_MS);
//fprintf(stderr,"eventDispatcher %d\n", delayMs);
if (delayMs < 0)
{
delayMs = 0;