diff --git a/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/corvuscorax/port_with-debugging-output.c b/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/corvuscorax/port_with-debugging-output.c
new file mode 100755
index 000000000..e4bf48700
--- /dev/null
+++ b/flight/PiOS.posix/posix/Libraries/FreeRTOS/Source/portable/GCC/Posix/corvuscorax/port_with-debugging-output.c
@@ -0,0 +1,1041 @@
+/*
+	Copyright (C) 2009 William Davy - william.davy@wittenstein.co.uk
+	Contributed to FreeRTOS.org V5.3.0.
+
+	This file is part of the FreeRTOS.org distribution.
+
+	FreeRTOS.org is free software; you can redistribute it and/or modify it
+	under the terms of the GNU General Public License (version 2) as published
+	by the Free Software Foundation and modified by the FreeRTOS exception.
+
+	FreeRTOS.org is distributed in the hope that it will be useful,	but WITHOUT
+	ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+	FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+	more details.
+
+	You should have received a copy of the GNU General Public License along
+	with FreeRTOS.org; if not, write to the Free Software Foundation, Inc., 59
+	Temple Place, Suite 330, Boston, MA  02111-1307  USA.
+
+	A special exception to the GPL is included to allow you to distribute a
+	combined work that includes FreeRTOS.org without being obliged to provide
+	the source code for any proprietary components.  See the licensing section
+	of http://www.FreeRTOS.org for full details.
+
+
+	***************************************************************************
+	*                                                                         *
+	* Get the FreeRTOS eBook!  See http://www.FreeRTOS.org/Documentation      *
+	*                                                                         *
+	* This is a concise, step by step, 'hands on' guide that describes both   *
+	* general multitasking concepts and FreeRTOS specifics. It presents and   *
+	* explains numerous examples that are written using the FreeRTOS API.     *
+	* Full source code for all the examples is provided in an accompanying    *
+	* .zip file.                                                              *
+	*                                                                         *
+	***************************************************************************
+
+	1 tab == 4 spaces!
+
+	Please ensure to read the configuration and relevant port sections of the
+	online documentation.
+
+	http://www.FreeRTOS.org - Documentation, latest information, license and
+	contact details.
+
+	http://www.SafeRTOS.com - A version that is certified for use in safety
+	critical systems.
+
+	http://www.OpenRTOS.com - Commercial support, development, porting,
+	licensing and training services.
+*/
+
+/*-----------------------------------------------------------
+ * Implementation of functions defined in portable.h for the Posix port.
+ *----------------------------------------------------------*/
+
+#include <pthread.h>
+#include <sched.h>
+#include <signal.h>
+#include <errno.h>
+#include <sys/time.h>
+#include <time.h>
+#include <sys/times.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <limits.h>
+
+/* Scheduler includes. */
+#include "FreeRTOS.h"
+#include "task.h"
+/*-----------------------------------------------------------*/
+
+#define MAX_NUMBER_OF_TASKS 		( _POSIX_THREAD_THREADS_MAX )
+/*-----------------------------------------------------------*/
+
+/* Parameters to pass to the newly created pthread. */
+typedef struct XPARAMS
+{
+	pdTASK_CODE pxCode;
+	void *pvParams;
+} xParams;
+
+/* Each task maintains its own interrupt status in the critical nesting variable. */
+typedef struct THREAD_SUSPENSIONS
+{
+	pthread_t hThread;
+	xTaskHandle hTask;
+	unsigned portBASE_TYPE uxCriticalNesting;
+} xThreadState;
+/*-----------------------------------------------------------*/
+
+static xThreadState *pxThreads;
+static pthread_once_t hSigSetupThread = PTHREAD_ONCE_INIT;
+static pthread_attr_t xThreadAttributes;
+static pthread_mutex_t xSuspendResumeThreadMutex = PTHREAD_MUTEX_INITIALIZER;
+//static pthread_mutex_t xSingleThreadMutex = PTHREAD_MUTEX_INITIALIZER;
+static pthread_mutex_t xRunningThreadMutex = PTHREAD_MUTEX_INITIALIZER;
+static pthread_mutex_t xPrintfMutex = PTHREAD_MUTEX_INITIALIZER;
+static pthread_t hMainThread = ( pthread_t )NULL;
+static pthread_t hActiveThread = ( pthread_t )NULL;
+static pthread_t hRequestedThread = ( pthread_t )NULL;
+/*-----------------------------------------------------------*/
+
+static volatile portBASE_TYPE xSentinel = 0;
+static volatile portBASE_TYPE xGeneralFuckedUpIndicator = 0;
+static volatile portBASE_TYPE xVeryFirstTask = 1;
+static volatile portBASE_TYPE xSchedulerEnd = pdFALSE;
+static volatile portBASE_TYPE xInterruptsEnabled = pdTRUE;
+static volatile portBASE_TYPE xServicingTick = pdFALSE;
+static volatile portBASE_TYPE xPendYield = pdFALSE;
+static volatile portLONG lIndexOfLastAddedTask = 0;
+static volatile unsigned portBASE_TYPE uxCriticalNesting;
+/*-----------------------------------------------------------*/
+
+/*
+ * Setup the timer to generate the tick interrupts.
+ */
+static void prvSetupTimerInterrupt( void );
+static void *prvWaitForStart( void * pvParams );
+static void prvSuspendSignalHandler(int sig);
+//static void prvResumeSignalHandler(int sig);
+static void prvSetupSignalsAndSchedulerPolicy( void );
+static void prvSuspendThread( pthread_t xThreadId );
+static void prvResumeThread( pthread_t xThreadId );
+static pthread_t prvGetThreadHandle( xTaskHandle hTask );
+static portLONG prvGetFreeThreadState( void );
+static void prvSetTaskCriticalNesting( pthread_t xThreadId, unsigned portBASE_TYPE uxNesting );
+static unsigned portBASE_TYPE prvGetTaskCriticalNesting( pthread_t xThreadId );
+static void prvDeleteThread( void *xThreadId );
+static void prvResolveFuckup( void );
+/*-----------------------------------------------------------*/
+typedef struct tskTaskControlBlock
+{
+	volatile portSTACK_TYPE	*pxTopOfStack;		/*< Points to the location of the last item placed on the tasks stack.  THIS MUST BE THE FIRST MEMBER OF THE STRUCT. */
+	
+#if ( portUSING_MPU_WRAPPERS == 1 )
+	xMPU_SETTINGS xMPUSettings;				/*< The MPU settings are defined as part of the port layer.  THIS MUST BE THE SECOND MEMBER OF THE STRUCT. */
+#endif	
+	
+	xListItem				xGenericListItem;	/*< List item used to place the TCB in ready and blocked queues. */
+	xListItem				xEventListItem;		/*< List item used to place the TCB in event lists. */
+	unsigned portBASE_TYPE	uxPriority;			/*< The priority of the task where 0 is the lowest priority. */
+	portSTACK_TYPE			*pxStack;			/*< Points to the start of the stack. */
+	signed char				pcTaskName[ configMAX_TASK_NAME_LEN ];/*< Descriptive name given to the task when created.  Facilitates debugging only. */
+	
+#if ( portSTACK_GROWTH > 0 )
+	portSTACK_TYPE *pxEndOfStack;			/*< Used for stack overflow checking on architectures where the stack grows up from low memory. */
+#endif
+	
+#if ( portCRITICAL_NESTING_IN_TCB == 1 )
+	unsigned portBASE_TYPE uxCriticalNesting;
+#endif
+	
+#if ( configUSE_TRACE_FACILITY == 1 )
+	unsigned portBASE_TYPE	uxTCBNumber;	/*< This is used for tracing the scheduler and making debugging easier only. */
+#endif
+	
+#if ( configUSE_MUTEXES == 1 )
+	unsigned portBASE_TYPE uxBasePriority;	/*< The priority last assigned to the task - used by the priority inheritance mechanism. */
+#endif
+	
+#if ( configUSE_APPLICATION_TASK_TAG == 1 )
+	pdTASK_HOOK_CODE pxTaskTag;
+#endif
+	
+#if ( configGENERATE_RUN_TIME_STATS == 1 )
+	unsigned long ulRunTimeCounter;		/*< Used for calculating how much CPU time each task is utilising. */
+#endif
+	
+} tskTCB;
+
+tskTCB* debug_task_handle;
+tskTCB* prvGetTaskHandle( pthread_t hThread )
+{
+portLONG lIndex;
+
+	if (pxThreads==NULL) return NULL;
+
+	for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
+	{
+		if ( pxThreads[ lIndex ].hThread == hThread )
+		{
+			return pxThreads[ lIndex ].hTask;
+		}
+	}
+	return NULL;
+}
+
+
+#define debug_printf(...) ( (real_pthread_mutex_lock( &xPrintfMutex )|1)?( \
+	(  \
+		(NULL != (debug_task_handle = prvGetTaskHandle(pthread_self())) )? \
+			(fprintf( stderr, "%s(%li)\t%s\t%i:",debug_task_handle->pcTaskName,(long)pthread_self(),__func__,__LINE__)): \
+			(fprintf( stderr, "__unknown__(%li)\t%s\t%i:",(long)pthread_self(),__func__,__LINE__)) \
+	|1)?( \
+			((fprintf( stderr, __VA_ARGS__ )|1)?real_pthread_mutex_unlock( &xPrintfMutex ):0) \
+	):0 ):0 )
+
+//int xbla;
+//#define debug_printf(...) xbla=0
+int real_pthread_mutex_lock(pthread_mutex_t* mutex) {
+	return pthread_mutex_lock(mutex);
+}
+int real_pthread_mutex_unlock(pthread_mutex_t* mutex) {
+	return pthread_mutex_unlock(mutex);
+}
+#define pthread_mutex_lock(...) ( (debug_printf(" -!- pthread_mutex_lock(%s)\n",#__VA_ARGS__)|1)?pthread_mutex_lock(__VA_ARGS__):0 )
+#define pthread_mutex_unlock(...) ( (debug_printf(" -=- pthread_mutex_unlock(%s)\n",#__VA_ARGS__)|1)?pthread_mutex_unlock(__VA_ARGS__):0 )
+#define pthread_kill(thread,signal) ( (debug_printf(" sending signal %i to thread %li!\n",(int)signal,(long)thread)|1)?pthread_kill(thread,signal):0 )
+#define vTaskSwitchContext() ( (debug_printf("SWITCHCONTEXT!\n")|1)?vTaskSwitchContext():vTaskSwitchContext() )
+/*-----------------------------------------------------------*/
+
+void prvSuspendThread( pthread_t xThreadId )
+{
+portBASE_TYPE xResult;
+//	xResult = pthread_mutex_lock( &xSuspendResumeThreadMutex );
+//	if ( 0 == xResult )
+//	{
+		/* Set-up for the Suspend Signal handler? */
+		//xSentinel = 0;
+//portBASE_TYPE aSentinel=xSentinel;
+		xResult = pthread_kill( xThreadId, SIG_SUSPEND );
+//		xResult = pthread_mutex_unlock( &xSuspendResumeThreadMutex );
+//		while ( ( aSentinel == xSentinel ) && ( pdTRUE != xServicingTick ) )
+//		{
+//			sched_yield();
+//		}
+//	}
+}
+/*-----------------------------------------------------------*/
+
+void prvResumeThread( pthread_t xThreadId )
+{
+portBASE_TYPE xResult;
+//	if ( 0 == pthread_mutex_lock( &xSuspendResumeThreadMutex ) )
+//	{
+		if ( pthread_self() != xThreadId )
+		{
+			xResult = pthread_kill( xThreadId, SIG_RESUME );
+		}
+//		xResult = pthread_mutex_unlock( &xSuspendResumeThreadMutex );
+//	}
+}
+#define prvSuspendThread(thread) debug_printf("calling SuspendThread(%li)\n",(long)thread); prvSuspendThread(thread)
+#define prvResumeThread(thread) debug_printf("calling ResumeThread(%li)\n",(long)thread); prvResumeThread(thread)
+
+/*
+ * Exception handlers.
+ */
+void vPortYield( void );
+void vPortSystemTickHandler( int sig );
+
+/*
+ * Start first task is a separate function so it can be tested in isolation.
+ */
+void vPortStartFirstTask( void );
+/*-----------------------------------------------------------*/
+
+/*
+ * See header file for description.
+ */
+portSTACK_TYPE *pxPortInitialiseStack( portSTACK_TYPE *pxTopOfStack, pdTASK_CODE pxCode, void *pvParameters )
+{
+/* Should actually keep this struct on the stack. */
+xParams *pxThisThreadParams = pvPortMalloc( sizeof( xParams ) );
+
+	(void)pthread_once( &hSigSetupThread, prvSetupSignalsAndSchedulerPolicy );
+
+	if ( (pthread_t)NULL == hMainThread )
+	{
+		hMainThread = pthread_self();
+	}
+
+	/* No need to join the threads. */
+	pthread_attr_init( &xThreadAttributes );
+	pthread_attr_setdetachstate( &xThreadAttributes, PTHREAD_CREATE_DETACHED );
+
+	/* Add the task parameters. */
+	pxThisThreadParams->pxCode = pxCode;
+	pxThisThreadParams->pvParams = pvParameters;
+
+	vPortEnterCritical();
+
+	lIndexOfLastAddedTask = prvGetFreeThreadState();
+
+	/* Create the new pThread. */
+	/* On creation of the very first thread, RunningThreadMutex is not claimed yet
+	 * by the master thread - do that! */
+	if (xVeryFirstTask==1) {
+		debug_printf("Seting up very first task (main) - MAIN is ACTIVE TASK\n");
+		if (0 == pthread_mutex_lock( &xRunningThreadMutex)) {
+			xVeryFirstTask=0;
+		} else {
+			printf("Failed to acquire lock for first task");
+			exit(1);
+		}
+
+	}
+
+	if ( 0 != pthread_create( &( pxThreads[ lIndexOfLastAddedTask ].hThread ), &xThreadAttributes, prvWaitForStart, (void *)pxThisThreadParams ) )
+	{
+		/* Thread create failed, signal the failure */
+		pxTopOfStack = 0;
+	}
+
+	/* Wait until the task suspends. */
+	xSentinel=0;
+	(void)pthread_mutex_unlock( &xRunningThreadMutex );
+	while ( xSentinel == 0 ) {
+		sched_yield();
+	}
+	(void)pthread_mutex_lock( &xRunningThreadMutex );
+	vPortExitCritical();
+
+	return pxTopOfStack;
+}
+/*-----------------------------------------------------------*/
+
+void vPortStartFirstTask( void )
+{
+	/* Initialise the critical nesting count ready for the first task. */
+	uxCriticalNesting = 0;
+
+	/* Start the first task. */
+	vPortEnableInterrupts();
+
+	/* Start the first task. */
+	hRequestedThread=prvGetThreadHandle( xTaskGetCurrentTaskHandle());
+	prvResumeThread( hRequestedThread );
+
+	sched_yield();
+}
+/*-----------------------------------------------------------*/
+
+/*
+ * See header file for description.
+ */
+portBASE_TYPE xPortStartScheduler( void )
+{
+portBASE_TYPE xResult;
+int iSignal;
+int fuckedUpCount=0;
+sigset_t xSignals;
+sigset_t xSignalToBlock;
+sigset_t xSignalsBlocked;
+portLONG lIndex;
+
+	/* Establish the signals to block before they are needed. */
+	sigfillset( &xSignalToBlock );
+
+	/* Block until the end */
+	(void)pthread_sigmask( SIG_SETMASK, &xSignalToBlock, &xSignalsBlocked );
+
+	for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
+	{
+		pxThreads[ lIndex ].uxCriticalNesting = 0;
+	}
+
+	/* Start the timer that generates the tick ISR.  Interrupts are disabled
+	here already. */
+	prvSetupTimerInterrupt();
+
+	/* Start the first task. Will not return unless all threads are killed. */
+	vPortStartFirstTask();
+
+	/* This is the end signal we are looking for. */
+	sigemptyset( &xSignals );
+	sigaddset( &xSignals, SIG_RESUME );
+	sigaddset( &xSignals, SIG_TICK );
+
+	/* Allow other threads to run */
+	(void)pthread_mutex_unlock( &xRunningThreadMutex );
+	debug_printf( "MAIN thread is entering main signal wait loop!\n");
+
+	while ( pdTRUE != xSchedulerEnd )
+	{
+		if ( 0 != sigwait( &xSignals, &iSignal ) )
+		{
+			printf( "Main thread spurious signal: %d\n", iSignal );
+		}
+		/**
+		 * Tick handler is called from here - AND ONLY FROM HERE
+		 * (needed for cygwin - but should work on all)
+		 */
+		if (iSignal==SIG_TICK) {
+			if (xGeneralFuckedUpIndicator!=0) {
+				fuckedUpCount++;
+				if (fuckedUpCount>10) {
+					fuckedUpCount=0;
+					prvResolveFuckup();
+				}
+			} else {
+				fuckedUpCount=0;
+			}
+			vPortSystemTickHandler(iSignal);
+		}
+		if (iSignal==SIG_RESUME && pdTRUE != xSchedulerEnd) {
+			debug_printf( "ALERT! Main received SIG_RESUME that was supposed to go elsewhere!");
+		}
+	}
+
+	printf( "Cleaning Up, Exiting.\n" );
+	/* Cleanup the mutexes */
+	//xResult = pthread_mutex_destroy( &xSuspendResumeThreadMutex );
+	//xResult = pthread_mutex_destroy( &xSingleThreadMutex );
+	xResult = pthread_mutex_destroy( &xRunningThreadMutex );
+	vPortFree( (void *)pxThreads );
+
+	/* Should not get here! */
+	return 0;
+}
+/*-----------------------------------------------------------*/
+
+void vPortEndScheduler( void )
+{
+portBASE_TYPE xNumberOfThreads;
+portBASE_TYPE xResult;
+	for ( xNumberOfThreads = 0; xNumberOfThreads < MAX_NUMBER_OF_TASKS; xNumberOfThreads++ )
+	{
+		if ( ( pthread_t )NULL != pxThreads[ xNumberOfThreads ].hThread )
+		{
+			/* Kill all of the threads, they are in the detached state. */
+			xResult = pthread_cancel( pxThreads[ xNumberOfThreads ].hThread );
+		}
+	}
+
+	/* Signal the scheduler to exit its loop. */
+	xSchedulerEnd = pdTRUE;
+	(void)pthread_kill( hMainThread, SIG_RESUME );
+}
+/*-----------------------------------------------------------*/
+
+void vPortYieldFromISR( void )
+{
+	/* Calling Yield from a Interrupt/Signal handler often doesn't work because the
+	 * xSingleThreadMutex is already owned by an original call to Yield. Therefore,
+	 * simply indicate that a yield is required soon.
+	 */
+	xPendYield = pdTRUE;
+}
+/*-----------------------------------------------------------*/
+
+void vPortEnterCritical( void )
+{
+	vPortDisableInterrupts();
+	uxCriticalNesting++;
+}
+/*-----------------------------------------------------------*/
+
+void vPortExitCritical( void )
+{
+	/* Check for unmatched exits. */
+	if ( uxCriticalNesting > 0 )
+	{
+		uxCriticalNesting--;
+	}
+
+	/* If we have reached 0 then re-enable the interrupts. */
+	if( uxCriticalNesting == 0 )
+	{
+		/* Have we missed ticks? This is the equivalent of pending an interrupt. */
+		if ( pdTRUE == xPendYield )
+		{
+			xPendYield = pdFALSE;
+			vPortYield();
+		}
+		vPortEnableInterrupts();
+	}
+}
+/*-----------------------------------------------------------*/
+
+void vPortYield( void )
+{
+pthread_t xTaskToSuspend;
+pthread_t xTaskToResume;
+
+	/**
+	 * Sentinel - do not change context while the running task is not equal the task supposed to run
+	 */
+	if ( 0 == pthread_mutex_lock( &xSuspendResumeThreadMutex ) )
+	{
+		/**
+		 * Make sure we don't create outdated resume signals
+		 */
+		while (hActiveThread!=hRequestedThread) {
+			sched_yield();
+		}
+		xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
+
+		vTaskSwitchContext();
+
+		xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
+		hRequestedThread = xTaskToResume;	
+		if ( xTaskToSuspend != xTaskToResume )
+		{
+			/* Remember and switch the critical nesting. */
+			prvSetTaskCriticalNesting( xTaskToSuspend, uxCriticalNesting );
+			uxCriticalNesting = prvGetTaskCriticalNesting( xTaskToResume );
+			/* Switch tasks. */
+			prvResumeThread( xTaskToResume );
+			//prvSuspendThread( xTaskToSuspend );
+if (prvGetThreadHandle(xTaskGetCurrentTaskHandle())!=xTaskToResume) {
+	debug_printf("\n     what the fuck???? someone else did a switchcontext?!?\n");
+}
+			(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
+			prvSuspendSignalHandler(SIG_SUSPEND);
+			return;
+		}
+		else
+		{
+			/* Yielding to self */
+			(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
+		}
+	}
+}
+/*-----------------------------------------------------------*/
+
+void vPortDisableInterrupts( void )
+{
+	xInterruptsEnabled = pdFALSE;
+}
+/*-----------------------------------------------------------*/
+
+void vPortEnableInterrupts( void )
+{
+	xInterruptsEnabled = pdTRUE;
+}
+/*-----------------------------------------------------------*/
+
+portBASE_TYPE xPortSetInterruptMask( void )
+{
+portBASE_TYPE xReturn = xInterruptsEnabled;
+	xInterruptsEnabled = pdFALSE;
+	return xReturn;
+}
+/*-----------------------------------------------------------*/
+
+void vPortClearInterruptMask( portBASE_TYPE xMask )
+{
+	xInterruptsEnabled = xMask;
+}
+/*-----------------------------------------------------------*/
+
+/*
+ * Setup the systick timer to generate the tick interrupts at the required
+ * frequency.
+ */
+void prvSetupTimerInterrupt( void )
+{
+struct itimerval itimer, oitimer;
+portTickType xMicroSeconds = portTICK_RATE_MICROSECONDS;
+
+	debug_printf("init %li microseconds\n",(long)xMicroSeconds);
+	/* Initialise the structure with the current timer information. */
+	if ( 0 == getitimer( TIMER_TYPE, &itimer ) )
+	{
+		/* Set the interval between timer events. */
+		itimer.it_interval.tv_sec = 0;
+		itimer.it_interval.tv_usec = xMicroSeconds;
+
+		/* Set the current count-down. */
+		itimer.it_value.tv_sec = 0;
+		itimer.it_value.tv_usec = xMicroSeconds;
+
+		/* Set-up the timer interrupt. */
+		if ( 0 != setitimer( TIMER_TYPE, &itimer, &oitimer ) )
+		{
+			printf( "Set Timer problem.\n" );
+		}
+	}
+	else
+	{
+		printf( "Get Timer problem.\n" );
+	}
+}
+/*-----------------------------------------------------------*/
+
+void vPortSystemTickHandler( int sig )
+{
+pthread_t xTaskToSuspend;
+pthread_t xTaskToResume;
+struct timespec timeout;
+
+	//debug_printf("received %i\n",sig);
+	/**
+	 * do not call tick handler if
+	 * - interrupts are disabled
+	 * - tick handler is still running
+	 * - old task switch not yet completed (wrong task running)
+	 */
+	if ( ( pdTRUE == xInterruptsEnabled ) && ( pdTRUE != xServicingTick ) && ( hRequestedThread == hActiveThread ) )
+	{
+		if ( 0 == pthread_mutex_trylock( &xSuspendResumeThreadMutex ) )
+		{
+			debug_printf("does handle tick\n");
+			xServicingTick = pdTRUE;
+			
+			/**
+			 * this shouldn't ever happen - but WELL...
+			 * Make sure we don't create outdated resume signals
+			 */
+			while (hActiveThread!=hRequestedThread) {
+				sched_yield();
+			}
+
+			xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
+			/* Tick Increment. */
+			vTaskIncrementTick();
+
+			/* Select Next Task. */
+#if ( configUSE_PREEMPTION == 1 )
+			vTaskSwitchContext();
+#endif
+			xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
+			hRequestedThread = xTaskToResume;	
+
+			/* The only thread that can process this tick is the running thread. */
+			if ( xTaskToSuspend != xTaskToResume )
+			{
+				/* Suspend the current task. */
+				prvSuspendThread( xTaskToSuspend );
+				timeout.tv_sec=0;
+				timeout.tv_nsec=10000;
+				//if ( 0 == pthread_mutex_timedlock( &xRunningThreadMutex,&timeout ) ) {
+				if ( 0 == pthread_mutex_lock( &xRunningThreadMutex) ) {
+					/* Remember and switch the critical nesting. */
+					prvSetTaskCriticalNesting( xTaskToSuspend, uxCriticalNesting );
+					uxCriticalNesting = prvGetTaskCriticalNesting( xTaskToResume );
+					/* Resume next task. */
+					prvResumeThread( xTaskToResume );
+if (prvGetThreadHandle(xTaskGetCurrentTaskHandle())!=xTaskToResume) {
+	debug_printf("\n     what the fuck???? someone else did a switchcontext?!?\n");
+}
+					(void)pthread_mutex_unlock( &xRunningThreadMutex );
+				} else {
+					debug_printf("Oh dear - tick handler could not acquire lock!\n\n");
+					//prvResumeThread( xTaskToSuspend );
+					xGeneralFuckedUpIndicator=3;
+				}
+			}
+			else
+			{
+				/* Release the lock as we are Resuming. */
+		//		(void)pthread_mutex_unlock( &xSingleThreadMutex );
+			}
+			xServicingTick = pdFALSE;
+		}
+		else
+		{
+			xPendYield = pdTRUE;
+		}
+		(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
+	}
+	else
+	{
+		debug_printf("will NOT handle tick\n");
+		xPendYield = pdTRUE;
+	}
+}
+/*-----------------------------------------------------------*/
+
+void vPortForciblyEndThread( void *pxTaskToDelete )
+{
+xTaskHandle hTaskToDelete = ( xTaskHandle )pxTaskToDelete;
+pthread_t xTaskToDelete;
+pthread_t xTaskToResume;
+portBASE_TYPE xResult;
+
+//	if ( 0 == pthread_mutex_lock( &xSingleThreadMutex ) )
+//	{
+		xTaskToDelete = prvGetThreadHandle( hTaskToDelete );
+		xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
+
+		if ( xTaskToResume == xTaskToDelete )
+		{
+			if ( 0 == pthread_mutex_lock( &xSuspendResumeThreadMutex ) ) {
+				/**
+				 * Make sure we don't create outdated resume signals
+				 */
+				while (hActiveThread!=hRequestedThread) {
+					sched_yield();
+				}
+				/* This is a suicidal thread, need to select a different task to run. */
+				vTaskSwitchContext();
+				xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
+				hRequestedThread = xTaskToResume;
+				(void)pthread_mutex_unlock( &xSuspendResumeThreadMutex );
+			} else {
+				debug_printf("THIS was never meant to happen\n");
+				exit(1);
+			}
+		}
+
+		if ( pthread_self() != xTaskToDelete )
+		{
+			/* Cancelling a thread that is not me. */
+			if ( xTaskToDelete != ( pthread_t )NULL )
+			{
+				/* Send a signal to wake the task so that it definitely cancels. */
+				pthread_testcancel();
+				xResult = pthread_cancel( xTaskToDelete );
+				/* Pthread Clean-up function will note the cancellation. */
+			}
+//			(void)pthread_mutex_unlock( &xSingleThreadMutex );
+		}
+		else
+		{
+			/* Resume the other thread. */
+			prvResumeThread( xTaskToResume );
+			/* Pthread Clean-up function will note the cancellation. */
+			/* Release the execution. */
+			uxCriticalNesting = 0;
+			vPortEnableInterrupts();
+//			(void)pthread_mutex_unlock( &xSingleThreadMutex );
+			(void)pthread_mutex_unlock( &xRunningThreadMutex );
+			/* Commit suicide */
+			pthread_exit( (void *)1 );
+		}
+//	}
+}
+/*-----------------------------------------------------------*/
+
+void *prvWaitForStart( void * pvParams )
+{
+xParams * pxParams = ( xParams * )pvParams;
+pdTASK_CODE pvCode = pxParams->pxCode;
+void * pParams = pxParams->pvParams;
+sigset_t xSignals;
+	vPortFree( pvParams );
+
+	pthread_cleanup_push( prvDeleteThread, (void *)pthread_self() );
+
+	if ( 0 == pthread_mutex_lock( &xRunningThreadMutex ) )
+	{
+		xSentinel=1;
+
+		sigemptyset( &xSignals );
+		sigaddset( &xSignals, SIG_RESUME );
+		/* set up proc mask */
+		pthread_sigmask(SIG_SETMASK,&xSignals,NULL);
+
+		prvSuspendSignalHandler(SIG_SUSPEND);
+		//prvSuspendThread( pthread_self() );
+	} else {
+		debug_printf("now this is just WRONG!\n");
+		exit(1);
+	}
+
+	pvCode( pParams );
+
+	pthread_cleanup_pop( 1 );
+	return (void *)NULL;
+}
+/*-----------------------------------------------------------*/
+
+void prvSuspendSignalHandler(int sig)
+{
+sigset_t xSignals;
+//sigset_t xPendingSignals;
+
+	/* Only interested in the resume signal. */
+	sigemptyset( &xSignals );
+	sigaddset( &xSignals, SIG_RESUME );
+	sigaddset( &xSignals, SIG_SUSPEND );
+
+	/* Unlock the Running thread mutex to allow the resumed task to continue. */
+	if ( 0 != pthread_mutex_unlock( &xRunningThreadMutex ) )
+	{
+		printf( "Releasing someone else's lock.\n" );
+	}
+
+	debug_printf("SUSPENDING until SIG_RESUME received\n");
+	/* Wait on the resume signal. */
+	while (hRequestedThread != pthread_self()) {
+		if ( 0 != sigwait( &xSignals, &sig ) )
+		{
+			printf( "SSH: Sw %d\n", sig );
+			/* tricky one - shouldn't ever happen - trying to resolve situation as graceful as possible */
+			debug_printf("ALERT AAAAH PANIC! - sigwait failed.....\n\n\n");
+			/* Signal main thread something just went HORRIBLY wrong */
+			xGeneralFuckedUpIndicator = 2;
+			//(void)pthread_mutex_lock( &xRunningThreadMutex );
+			//(void)pthread_kill( pthread_self(), SIG_SUSPEND );
+			//return;
+		} else if (sig == SIG_RESUME) {
+			//debug_printf("received signal %i\n",sig);
+			/* Make sure the right thread received the signal */
+			if (hRequestedThread != pthread_self() ) {
+				debug_printf( "ALERT! Received SIG_RESUME which is already outdated!\n     active thread is %li\n",(long)hRequestedThread);
+				/* Signal main thread something just went wrong */
+				xGeneralFuckedUpIndicator = 1;
+				/*
+				if (0 == sigpending(&xPendingSignals)) {
+					if (sigismember(&xPendingSignals,SIG_SUSPEND)) {
+						debug_printf( "reason: we slept too long...\n");
+						//(void)sigwait(&xPendingSignals,&sig);
+						// we can safely return - signal is already pending
+						return;
+					}
+				}
+				debug_printf( "reason: unknown! - whatever ...\n\n");
+				*/
+				//exit(1);
+				//(void)pthread_kill( xTaskToResume, SIG_RESUME );
+				//(void)pthread_mutex_lock( &xRunningThreadMutex );
+				//(void)pthread_kill( pthread_self(), SIG_SUSPEND );
+				//return;
+			}
+		}
+	}
+	/* Yield the Scheduler to ensure that the yielding thread completes. */
+	if ( 0 != pthread_mutex_lock( &xRunningThreadMutex ) )
+	{
+		//(void)pthread_mutex_unlock( &xSingleThreadMutex );
+		debug_printf("critical - mutex acquiring of active thread failed!\n");
+		exit(1);
+	}
+	hActiveThread = pthread_self();
+
+	/* Will resume here when the SIG_RESUME signal is received. */
+	/* Need to set the interrupts based on the task's critical nesting. */
+	if ( uxCriticalNesting == 0 )
+	{
+		vPortEnableInterrupts();
+	}
+	else
+	{
+		vPortDisableInterrupts();
+	}
+	if (hRequestedThread==pthread_self()) {
+		/* doesn't look too bad, does it? */
+		xGeneralFuckedUpIndicator = 0;
+	}
+	debug_printf("ACTIVE THREAD!\n");
+}
+/*-----------------------------------------------------------*/
+
+void prvSetupSignalsAndSchedulerPolicy( void )
+{
+/* The following code would allow for configuring the scheduling of this task as a Real-time task.
+ * The process would then need to be run with higher privileges for it to take affect.
+int iPolicy;
+int iResult;
+int iSchedulerPriority;
+	iResult = pthread_getschedparam( pthread_self(), &iPolicy, &iSchedulerPriority );
+	iResult = pthread_attr_setschedpolicy( &xThreadAttributes, SCHED_FIFO );
+	iPolicy = SCHED_FIFO;
+	iResult = pthread_setschedparam( pthread_self(), iPolicy, &iSchedulerPriority );		*/
+
+struct sigaction sigsuspendself, sigresume, sigtick;
+portLONG lIndex;
+
+	pxThreads = ( xThreadState *)pvPortMalloc( sizeof( xThreadState ) * MAX_NUMBER_OF_TASKS );
+	for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
+	{
+		pxThreads[ lIndex ].hThread = ( pthread_t )NULL;
+		pxThreads[ lIndex ].hTask = ( xTaskHandle )NULL;
+		pxThreads[ lIndex ].uxCriticalNesting = 0;
+	}
+
+	sigsuspendself.sa_flags = 0;
+	sigsuspendself.sa_handler = prvSuspendSignalHandler;
+	sigfillset( &sigsuspendself.sa_mask );
+
+	sigresume.sa_flags = 0;
+	//sigresume.sa_handler = prvResumeSignalHandler;
+	sigresume.sa_handler = SIG_IGN;
+	sigfillset( &sigresume.sa_mask );
+
+	sigtick.sa_flags = 0;
+	//sigtick.sa_handler = vPortSystemTickHandler;
+	sigtick.sa_handler = SIG_IGN;
+	sigfillset( &sigtick.sa_mask );
+
+	if ( 0 != sigaction( SIG_SUSPEND, &sigsuspendself, NULL ) )
+	{
+		printf( "Problem installing SIG_SUSPEND_SELF\n" );
+	}
+	//if ( 0 != sigaction( SIG_RESUME, &sigresume, NULL ) )
+	//{
+	//	printf( "Problem installing SIG_RESUME\n" );
+	//}
+	if ( 0 != sigaction( SIG_TICK, &sigtick, NULL ) )
+	{
+		printf( "Problem installing SIG_TICK\n" );
+	}
+	printf( "Running as PID: %d\n", getpid() );
+}
+/*-----------------------------------------------------------*/
+
+pthread_t prvGetThreadHandle( xTaskHandle hTask )
+{
+pthread_t hThread = ( pthread_t )NULL;
+portLONG lIndex;
+	for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
+	{
+		if ( pxThreads[ lIndex ].hTask == hTask )
+		{
+			hThread = pxThreads[ lIndex ].hThread;
+			break;
+		}
+	}
+	return hThread;
+}
+/*-----------------------------------------------------------*/
+
+portLONG prvGetFreeThreadState( void )
+{
+portLONG lIndex;
+	for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
+	{
+		if ( pxThreads[ lIndex ].hThread == ( pthread_t )NULL )
+		{
+			break;
+		}
+	}
+
+	if ( MAX_NUMBER_OF_TASKS == lIndex )
+	{
+		printf( "No more free threads, please increase the maximum.\n" );
+		lIndex = 0;
+		vPortEndScheduler();
+	}
+
+	return lIndex;
+}
+/*-----------------------------------------------------------*/
+
+void prvSetTaskCriticalNesting( pthread_t xThreadId, unsigned portBASE_TYPE uxNesting )
+{
+portLONG lIndex;
+	for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
+	{
+		if ( pxThreads[ lIndex ].hThread == xThreadId )
+		{
+			pxThreads[ lIndex ].uxCriticalNesting = uxNesting;
+			break;
+		}
+	}
+}
+/*-----------------------------------------------------------*/
+
+unsigned portBASE_TYPE prvGetTaskCriticalNesting( pthread_t xThreadId )
+{
+unsigned portBASE_TYPE uxNesting = 0;
+portLONG lIndex;
+	for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
+	{
+		if ( pxThreads[ lIndex ].hThread == xThreadId )
+		{
+			uxNesting = pxThreads[ lIndex ].uxCriticalNesting;
+			break;
+		}
+	}
+	return uxNesting;
+}
+/*-----------------------------------------------------------*/
+
+void prvDeleteThread( void *xThreadId )
+{
+portLONG lIndex;
+	for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
+	{
+		if ( pxThreads[ lIndex ].hThread == ( pthread_t )xThreadId )
+		{
+			pxThreads[ lIndex ].hThread = (pthread_t)NULL;
+			pxThreads[ lIndex ].hTask = (xTaskHandle)NULL;
+			if ( pxThreads[ lIndex ].uxCriticalNesting > 0 )
+			{
+				uxCriticalNesting = 0;
+				vPortEnableInterrupts();
+			}
+			pxThreads[ lIndex ].uxCriticalNesting = 0;
+			break;
+		}
+	}
+}
+/*-----------------------------------------------------------*/
+
+void vPortAddTaskHandle( void *pxTaskHandle )
+{
+portLONG lIndex;
+
+	pxThreads[ lIndexOfLastAddedTask ].hTask = ( xTaskHandle )pxTaskHandle;
+	for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
+	{
+		if ( pxThreads[ lIndex ].hThread == pxThreads[ lIndexOfLastAddedTask ].hThread )
+		{
+			if ( pxThreads[ lIndex ].hTask != pxThreads[ lIndexOfLastAddedTask ].hTask )
+			{
+				pxThreads[ lIndex ].hThread = ( pthread_t )NULL;
+				pxThreads[ lIndex ].hTask = NULL;
+				pxThreads[ lIndex ].uxCriticalNesting = 0;
+			}
+		}
+	}
+}
+/*-----------------------------------------------------------*/
+
+void vPortFindTicksPerSecond( void )
+{
+	/* Needs to be reasonably high for accuracy. */
+	unsigned long ulTicksPerSecond = sysconf(_SC_CLK_TCK);
+	printf( "Timer Resolution for Run TimeStats is %ld ticks per second.\n", ulTicksPerSecond );
+}
+/*-----------------------------------------------------------*/
+
+unsigned long ulPortGetTimerValue( void )
+{
+struct tms xTimes;
+	unsigned long ulTotalTime = times( &xTimes );
+	/* Return the application code times.
+	 * The timer only increases when the application code is actually running
+	 * which means that the total execution times should add up to 100%.
+	 */
+	return ( unsigned long ) xTimes.tms_utime;
+
+	/* Should check ulTotalTime for being clock_t max minus 1. */
+	(void)ulTotalTime;
+}
+/*-----------------------------------------------------------*/
+/**
+ * Scheduler f***d up - we got to fix that
+ */
+void prvResolveFuckup( void )
+{
+
+	printf("Scheduler fucked up again - lets try to fix it...\n");
+	printf("sending sig_suspend to thread that is supposed to be dead...\n");
+	prvSuspendThread(hActiveThread);
+	printf("acquire running lock...");
+	if ( 0 == pthread_mutex_lock( &xRunningThreadMutex) ) {
+		printf("sending sig_resume to thread that is supposed to be running...\n");
+		prvResumeThread(hRequestedThread);
+		printf("giving up mutex...\n");
+		(void)pthread_mutex_unlock(&xRunningThreadMutex); 
+	}
+
+}
+/*-----------------------------------------------------------*/