Check in WIP for osx simulation

flight/Modules/Attitude/revolution/attitude.c

@@ -209,6 +209,15 @@ 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/Sensors/simulated/sensors.c

@@ -122,6 +122,7 @@ MODULE_INITCALL(SensorsInitialize, SensorsStart)
 /**
  * Simulated sensor task.  Run a model of the airframe and produce sensor values
  */
+int sensors_count;
 static void SensorsTask(void *parameters)
 {
 	portTickType lastSysTime;
@@ -139,19 +140,23 @@ static void SensorsTask(void *parameters)
 	homeLocation.Set = HOMELOCATION_SET_TRUE;
 	HomeLocationSet(&homeLocation);
 
-	sensor_sim_type = MODEL_QUADCOPTER;
+	sensor_sim_type = MODEL_AGNOSTIC;
 
 	// Main task loop
 	lastSysTime = xTaskGetTickCount();
 	uint32_t last_time = PIOS_DELAY_GetRaw();
 	while (1) {
 		PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
-
-		float dT = PIOS_DELAY_DiffuS(last_time) / 1.0e6;
-		if(dT > 0.010) {
-			fprintf(stderr,"Long sensor update\n");
+		
+		static int i;
+		i++;
+		if (i % 5000 == 0) {
+			float dT = PIOS_DELAY_DiffuS(last_time) / 10.0e6;
+			fprintf(stderr, "Sensor relative timing: %f\n", dT);
+			last_time = PIOS_DELAY_GetRaw();
 		}
-		last_time = PIOS_DELAY_GetRaw();
+		
+		sensors_count++;
 
 		switch(sensor_sim_type) {
 			case CONSTANT:

flight/Modules/Stabilization/stabilization.c

@@ -138,6 +138,8 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart)
 /**
  * Module task
  */
+int stabilization_step;
+int stabilization_count;
 static void stabilizationTask(void* parameters)
 {
 	UAVObjEvent ev;
@@ -158,6 +160,19 @@ 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 )
 		{
@@ -165,6 +180,8 @@ static void stabilizationTask(void* parameters)
 			continue;
 		}
 
+		stabilization_step = 2;
+
 		dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f;
 		timeval = PIOS_DELAY_GetRaw();
 
@@ -172,6 +189,8 @@ static void stabilizationTask(void* parameters)
 		StabilizationDesiredGet(&stabDesired);
 		AttitudeActualGet(&attitudeActual);
 		GyrosGet(&gyrosData);
+		
+		stabilization_step = 3;
 
 #if defined(DIAGNOSTICS)
 		RateDesiredGet(&rateDesired);
@@ -184,6 +203,8 @@ 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;
@@ -206,6 +227,8 @@ 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,
@@ -222,6 +245,8 @@ 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++)
@@ -289,6 +314,8 @@ static void stabilizationTask(void* parameters)
 			}
 		}
 
+		stabilization_step = 7;
+
 		uint8_t shouldUpdate = 1;
 #if defined(DIAGNOSTICS)
 		RateDesiredSet(&rateDesired);
@@ -335,6 +362,8 @@ static void stabilizationTask(void* parameters)
 			}
 		}
 
+		stabilization_step = 8;
+
 		// Save dT
 		actuatorDesired.UpdateTime = dT * 1000;
 
@@ -349,6 +378,8 @@ static void stabilizationTask(void* parameters)
 			ActuatorDesiredSet(&actuatorDesired);
 		}
 
+		stabilization_step = 9;
+
 		if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
 		   (lowThrottleZeroIntegral && stabDesired.Throttle < 0) ||
 		   !shouldUpdate)
@@ -356,6 +387,7 @@ 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 1000
+#define SYSTEM_UPDATE_PERIOD_MS 5000
 #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, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS);
+			vTaskDelayUntil(&lastSysTime, 50); //SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS);
 		}
 	}
 }

flight/Modules/Telemetry/telemetry.c

@@ -274,6 +274,7 @@ static void telemetryTxTask(void *parameters)
 	while (1) {
 		// Wait for queue message
 		if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) {
+			//fprintf(stderr,".");
 			// Process event
 			processObjEvent(&ev);
 		}
@@ -292,6 +293,8 @@ static void telemetryTxPriTask(void *parameters)
 	while (1) {
 		// Wait for queue message
 		if (xQueueReceive(priorityQueue, &ev, portMAX_DELAY) == pdTRUE) {
+			//fprintf(stderr,"*");
+
 			// Process event
 			processObjEvent(&ev);
 		}
@@ -308,6 +311,7 @@ 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/PiOS.osx/inc/pios_tcp_priv.h

@@ -36,6 +36,7 @@
 #include <unistd.h>
 #include <fcntl.h>
 #include <netinet/in.h>
+#include "fifo_buffer.h"
 
 struct pios_tcp_cfg {
 	const char *ip;
@@ -60,6 +61,7 @@ typedef struct {
 	pios_com_callback rx_in_cb;
 	uint32_t rx_in_context;
 	
+	t_fifo_buffer rx_fifo;
 	uint8_t rx_buffer[PIOS_TCP_RX_BUFFER_SIZE];
 	uint8_t tx_buffer[PIOS_TCP_RX_BUFFER_SIZE];
 } pios_tcp_dev;

flight/PiOS.osx/osx/Libraries/FreeRTOS/Source/portable/GCC/Posix/port.c

@@ -452,8 +452,8 @@ sigset_t xSignalToBlock;
 		i++;
 		if (i % 1000 == 0)
 			fprintf(stderr,".");
-		if (i % 5000 == 0)
-			printTasks();
+		//if (i % 5000 == 0)
+		//	printTasks();
 	}
 	
 	debug_printf( "Cleaning Up, Exiting.\n" );
@@ -1197,7 +1197,7 @@ void pauseSelf()
 static int pauseOtherThread(xTaskHandle hTask)
 {
 	const int MAX_TIME = 10000; // us
-	const int MAX_ATTEMPTS = 5;
+	const int MAX_ATTEMPTS = 100;
 	
 	assert(xInterruptsEnabled == pdTRUE);
 
@@ -1280,6 +1280,7 @@ static void resumeThread(xTaskHandle hTask)
  */
 tskTCB *lastClaim;
 int lastClaimType;
+int claim_count = 0;
 static void claimRunningSemaphore(int source)
 {
 	//fprintf(stderr,"Claimed the semaphore(%d) %s\r\n", source, threadToName(pthread_self()));
@@ -1293,20 +1294,24 @@ static void claimRunningSemaphore(int source)
 	
 	lastClaim = (tskTCB *) hTask;
 	lastClaimType = source;
+	claim_count ++;
 }
 
 /**
  * Claims the running semaphore or fails
  */
 tskTCB *lastRelease;
+int release_count = 0;
 static void releaseRunningSemaphore()
 {
 	assert( 0 == pthread_mutex_unlock( &xRunningThread ) ); 
 	//fprintf(stderr,"Released the semaphore %s\r\n", threadToName(pthread_self()));
 	
 	xTaskHandle hTask = prvGetTaskHandle( pthread_self() );
-	lastRelease = (tskTCB *) hTask; 
+	assert( lastClaim == hTask );
+	lastRelease = (tskTCB *) hTask;
 
+	release_count++;
 }
 
 

flight/PiOS.osx/osx/Libraries/FreeRTOS/Source/task.c

@@ -61,6 +61,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <unistd.h>
 
 /* Defining MPU_WRAPPERS_INCLUDED_FROM_API_FILE prevents task.h from redefining
 all the API functions to use the MPU wrappers.  That should only be done when
@@ -1813,6 +1814,8 @@ portTickType xTimeToWake;
 #endif /* configUSE_TIMERS */
 /*-----------------------------------------------------------*/
 
+#include "assert.h"
+extern volatile portBASE_TYPE xInterruptsEnabled;
 signed portBASE_TYPE xTaskRemoveFromEventList( const xList * const pxEventList )
 {
 tskTCB *pxUnblockedTCB;
@@ -1821,6 +1824,8 @@ portBASE_TYPE xReturn;
 	/* THIS FUNCTION MUST BE CALLED WITH INTERRUPTS DISABLED OR THE
 	SCHEDULER SUSPENDED.  It can also be called from within an ISR. */
 
+	assert( uxSchedulerSuspended || xInterruptsEnabled == pdFALSE);
+
 	/* The event list is sorted in priority order, so we can remove the
 	first in the list, remove the TCB from the delayed list, and add
 	it to the ready list.
@@ -1985,10 +1990,8 @@ static portTASK_FUNCTION( prvIdleTask, pvParameters )
 		}
 		#endif
 		
-		static int i = 0;
-		i++;
-		if (i % 100000 == 0)
-			printTasks();
+		
+		//usleep(5);
 	}
 } /*lint !e715 pvParameters is not accessed but all task functions require the same prototype. */
 

flight/PiOS.osx/osx/pios_tcp.c

@@ -81,13 +81,12 @@ static void PIOS_TCP_RxTask(void *tcp_dev_n)
 
 	pios_tcp_dev *tcp_dev = (pios_tcp_dev *) tcp_dev_n;
 	while(1) {
-		if (tcp_dev->received) {
+		
+		if (tcp_dev->rx_in_cb) {
+			char buffer[PIOS_TCP_RX_BUFFER_SIZE];
+			int received = fifoBuf_getData(&tcp_dev->rx_fifo, buffer, PIOS_TCP_RX_BUFFER_SIZE);
+			(void) (tcp_dev->rx_in_cb)(tcp_dev->rx_in_context, (uint8_t *) buffer, received, NULL, &rx_need_yield);
 
-			if (tcp_dev->rx_in_cb) {
-				(void) (tcp_dev->rx_in_cb)(tcp_dev->rx_in_context, &tcp_dev->rx_buffer[0], tcp_dev->received, NULL, &rx_need_yield);
-				tcp_dev->received = 0;
-			}
-			
 			//fprintf(stderr, "Received %d\n", received);
 #if defined(PIOS_INCLUDE_FREERTOS)
 			// Not sure about this
@@ -95,9 +94,11 @@ static void PIOS_TCP_RxTask(void *tcp_dev_n)
 				vPortYieldFromISR();
 			}
 #endif	/* PIOS_INCLUDE_FREERTOS */
-
-			vTaskDelay(1);
-		}
+			
+		} else
+			fifoBuf_clearData(&tcp_dev->rx_fifo);	
+		
+		vTaskDelay(1);
 	}
 }
 
@@ -111,6 +112,8 @@ static void *PIOS_TCP_RxThread(void *tcp_dev_n)
 	
 	pios_tcp_dev *tcp_dev = (pios_tcp_dev*) tcp_dev_n;
 	
+	const int INCOMING_BUFFER_SIZE = 16;
+	char incoming_buffer[INCOMING_BUFFER_SIZE];
 	/**
 	 * com devices never get closed except by application "reboot"
 	 * we also never give up our mutex except for waiting
@@ -128,13 +131,13 @@ static void *PIOS_TCP_RxThread(void *tcp_dev_n)
 		
 		int received;
 		do {
-			if (tcp_dev->received == 0) {
-				// Received is used to track the scoket whereas the dev variable is only updated when it can be
-				received = read(tcp_dev->socket_connection, &tcp_dev->rx_buffer[0], PIOS_UDP_RX_BUFFER_SIZE);
-				tcp_dev->received = received;
-			} else
-				usleep(1);
+			// Received is used to track the scoket whereas the dev variable is only updated when it can be
+			received = read(tcp_dev->socket_connection, incoming_buffer, INCOMING_BUFFER_SIZE);
 			
+			while(fifoBuf_getFree(&tcp_dev->rx_fifo) < received)
+				usleep(10);
+			
+			fifoBuf_putData(&tcp_dev->rx_fifo, incoming_buffer, received);
 		} while(received > 0);
 		
 		if (-1 == shutdown(tcp_dev->socket_connection, SHUT_RDWR))
@@ -185,6 +188,8 @@ int32_t PIOS_TCP_Init(uint32_t *tcp_id, const struct pios_tcp_cfg * cfg)
 		exit(EXIT_FAILURE);
 	}
 	
+	fifoBuf_init(&tcp_dev->rx_fifo, tcp_dev->rx_buffer, PIOS_TCP_RX_BUFFER_SIZE);
+	
 	pthread_create(&tcp_dev->rxThread, NULL, PIOS_TCP_RxThread, (void*)tcp_dev);
 
 	xTaskCreate(PIOS_TCP_RxTask, (signed char *)"TcpRx", 1024, (void*)tcp_dev, 2, &tcpRxTaskHandle);

flight/Revolution/Makefile.osx

@@ -187,6 +187,7 @@ SRC += $(RTOSSRCDIR)/list.c
 SRC += $(RTOSSRCDIR)/queue.c
 UNAME := $(shell uname)
 SRC += $(RTOSSRCDIR)/task.c
+SRC += $(RTOSSRCDIR)/timers.c
 SRC += $(RTOSSRCDIR)/portable/GCC/Posix/port.c
 SRC += $(RTOSSRCDIR)/portable/MemMang/heap_3.c
 

flight/Revolution/System/inc/FreeRTOSConfig.h

@@ -43,7 +43,7 @@
 #define configCHECK_FOR_STACK_OVERFLOW			2
 #define configQUEUE_REGISTRY_SIZE				10
 
-#define configUSE_TIMERS						1
+#define configUSE_TIMERS						0
 #define configTIMER_TASK_PRIORITY				(configMAX_PRIORITIES - 1)	/* run timers at max priority */
 #define configTIMER_QUEUE_LENGTH				10
 #define configTIMER_TASK_STACK_DEPTH			configMINIMAL_STACK_SIZE

flight/Revolution/System/pios_board_sim.c

@@ -52,7 +52,7 @@ void Stack_Change_Weak() {
 
 const struct pios_tcp_cfg pios_tcp_telem_cfg = {
   .ip = "0.0.0.0",
-  .port = 9000,
+  .port = 9001,
 };
 const struct pios_tcp_cfg pios_tcp_gps_cfg = {
   .ip = "0.0.0.0",

flight/UAVObjects/eventdispatcher.c

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