diff --git a/flight/Modules/Attitude/revolution/attitude.c b/flight/Modules/Attitude/revolution/attitude.c index d30c6e8f9..915380c60 100644 --- a/flight/Modules/Attitude/revolution/attitude.c +++ b/flight/Modules/Attitude/revolution/attitude.c @@ -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: diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 62696f334..34a89870f 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -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); diff --git a/flight/Modules/System/systemmod.c b/flight/Modules/System/systemmod.c index b28e19df9..bf3ae7b01 100644 --- a/flight/Modules/System/systemmod.c +++ b/flight/Modules/System/systemmod.c @@ -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); } } } diff --git a/flight/Modules/Telemetry/telemetry.c b/flight/Modules/Telemetry/telemetry.c index db2dbfee7..01f74ab02 100644 --- a/flight/Modules/Telemetry/telemetry.c +++ b/flight/Modules/Telemetry/telemetry.c @@ -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) { diff --git a/flight/UAVObjects/eventdispatcher.c b/flight/UAVObjects/eventdispatcher.c index 1ff29f66d..c55726669 100644 --- a/flight/UAVObjects/eventdispatcher.c +++ b/flight/UAVObjects/eventdispatcher.c @@ -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;