Bring back the original module files and remove comments from FreeRTOS

debugging

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:

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();
 
@@ -190,8 +173,6 @@ static void stabilizationTask(void* parameters)
 		AttitudeActualGet(&attitudeActual);
 		GyrosGet(&gyrosData);
 
-		stabilization_step = 3;
-
 #if defined(DIAGNOSTICS)
 		RateDesiredGet(&rateDesired);
 #endif
@@ -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,
@@ -246,8 +223,6 @@ static void stabilizationTask(void* parameters)
 		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);

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);
 		}
 	}
 }

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) {

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;