PiOS.win32: Add MMCSS support for Vista onward, and smarter priority boosting to XP. Also, add the Actuator module and the fake pios_servo.c. If you run this using Windows 7 or Vista, then you need to run OpenPilot.exe as administrator once to set up the registry.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2270 ebee16cc-31ac-478f-84a7-5cbb03baadba

flight/OpenPilot/Makefile.win32

@@ -53,7 +53,7 @@ FLASH_TOOL = OPENOCD
 USE_THUMB_MODE = YES
 
 # List of modules to include
-MODULES =  Telemetry Stabilization ManualControl
+MODULES =  Telemetry Actuator Stabilization Guidance ManualControl
 #MODULES =  Telemetry GPS ManualControl Actuator Altitude Attitude Stabilization
 #MODULES = Telemetry Example
 #MODULES = Telemetry MK/MKSerial
@@ -172,6 +172,7 @@ SRC += $(PIOSWIN32)/pios_delay.c
 SRC += $(PIOSWIN32)/pios_sdcard.c
 SRC += $(PIOSWIN32)/pios_udp.c
 SRC += $(PIOSWIN32)/pios_com.c
+SRC += $(PIOSWIN32)/pios_servo.c
 #
 ## RTOS
 SRC += $(RTOSSRCDIR)/list.c 

flight/PiOS.win32/inc/pios_posix.h

@@ -30,5 +30,8 @@
 
 #define FILEINFO FILE*
 
+#define PIOS_SERVO_NUM_OUTPUTS 8
+#define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS
+
 #endif
 

flight/PiOS.win32/inc/pios_servo.h

@@ -0,0 +1,43 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup   PIOS_SERVO RC Servo Functions
+ * @{
+ *
+ * @file       pios_servo.h  
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      RC Servo functions header.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/* 
+ * This program is free software; you can redistribute it and/or modify 
+ * it under the terms of the GNU General Public License as published by 
+ * the Free Software Foundation; either version 3 of the License, or 
+ * (at your option) any later version.
+ * 
+ * This program 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 this program; if not, write to the Free Software Foundation, Inc., 
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef PIOS_SERVO_H
+#define PIOS_SERVO_H
+
+/* Public Functions */
+extern void PIOS_Servo_Init(void);
+extern void PIOS_Servo_SetHz(uint16_t onetofour, uint16_t fivetoeight);
+extern void PIOS_Servo_Set(uint8_t Servo, uint16_t Position);
+
+#endif /* PIOS_SERVO_H */
+
+/**
+  * @}
+  * @}
+  */

flight/PiOS.win32/pios.h

@@ -51,13 +51,14 @@
 /* PIOS Board Specific Device Configuration */
 #include "pios_board_posix.h"
 
-/* PIOS Hardware Includes (posix) */
+/* PIOS Hardware Includes (win32) */
 #include <pios_sys.h>
 #include <pios_delay.h>
 #include <pios_led.h>
 #include <pios_sdcard.h>
 #include <pios_udp.h>
 #include <pios_com.h>
+#include <pios_servo.h>
 
 #define NELEMENTS(x) (sizeof(x) / sizeof(*(x)))
 

flight/PiOS.win32/win32/Libraries/FreeRTOS/Source/portable/GCC/Win32/port.c

@@ -36,6 +36,7 @@
 /* Scheduler includes. */
 #include "FreeRTOS.h"
 #include "task.h"
+#include <shlobj.h>
 
 extern DWORD * pxCurrentTCB;
 
@@ -43,6 +44,8 @@ extern DWORD * pxCurrentTCB;
 	Defines
 ******************************************************************************/
 #define NMI	(1<<CPU_INTR_SWI)
+#define MAX_KEY_LENGTH 255
+
 
 /*
  * Task control block.  A task control block (TCB) is allocated to each task,
@@ -90,6 +93,22 @@ typedef struct tskTaskControlBlock
 
 tskTCB *debug_task_handle;
 
+//MinGW doesn't include MMCSS ...
+//Bummer!
+
+typedef enum _AVRT_PRIORITY
+{
+	AVRT_PRIORITY_LOW = -1,
+	AVRT_PRIORITY_NORMAL,
+	AVRT_PRIORITY_HIGH,
+	AVRT_PRIORITY_CRITICAL
+} AVRT_PRIORITY, *PAVRT_PRIORITY;
+
+typedef HANDLE WINAPI (*set_mm_thread_characteristics)(char *TaskName, LPDWORD TaskIndex);
+typedef BOOL WINAPI (*set_mm_thread_priority)(HANDLE AvrtHandle, AVRT_PRIORITY priority);
+
+set_mm_thread_characteristics AvSetMmThreadCharacteristics;
+set_mm_thread_priority AvSetMmThreadPriority;
 
 /*
 	Win32 simulator doesn't really use a stack. Instead It just
@@ -108,7 +127,7 @@ typedef struct
 
 #define portNO_CRITICAL_NESTING 		( ( unsigned portLONG ) 0 )
 
-#define DEBUG_OUTPUT
+//#define DEBUG_OUTPUT
 //#define ERROR_OUTPUT
 
 #ifdef DEBUG_OUTPUT
@@ -154,6 +173,8 @@ typedef struct
 	#endif
 #endif
 
+typedef BOOL (WINAPI *LPFN_ISWOW64PROCESS) (HANDLE, PBOOL);
+
 /******************************************************************************
 	National prototypes
 ******************************************************************************/
@@ -175,6 +196,9 @@ static volatile portSTACK_TYPE dwEnabledIsr = NMI;	// mask of enabled ISRs (indi
 static HANDLE hTickAck;		// acknolwledge tick interrupt
 static HANDLE hTermAck;		// acknowledge termination
 
+BOOL bIsWow64;
+BOOL bUsingMMCSS;
+
 static enum
 {
 	SWI_ID_YIELD,
@@ -221,6 +245,19 @@ static DWORD WINAPI tick_generator(LPVOID lpParameter)
 	HANDLE hObjList[2];
 	float before, after;
 
+	if(bUsingMMCSS)
+	{
+		DWORD taskIndex = 0;
+		HANDLE AvRtHandle = AvSetMmThreadCharacteristics(TEXT("FreeRTOS"), &taskIndex);
+		if(AvRtHandle == NULL)
+		{
+			printf("Error setting MMCSS on the tick generator.");
+			getchar();
+			exit(1);
+		}
+		AvSetMmThreadPriority(AvRtHandle, AVRT_PRIORITY_CRITICAL);
+	}
+
 	hTimer = CreateWaitableTimer(NULL, TRUE, NULL);
 	liDueTime.QuadPart = -(50000 - 10000*(int)msPerTick);	// 5ms -
 	//there is always another tick during WaitForMultipleObjects() while waiting
@@ -262,13 +299,283 @@ static DWORD WINAPI TaskSimThread( LPVOID lpParameter )
 	SSIM_T *psSim=(SSIM_T*)lpParameter;
 	ulCriticalNesting = psSim->ulCriticalNesting;
 
+	if(bUsingMMCSS)
+	{
+		DWORD taskIndex = 0;
+		HANDLE AvRtHandle = AvSetMmThreadCharacteristics("FreeRTOS", &taskIndex);
+		if(AvRtHandle == NULL)
+		{
+			printf("Error setting MMCSS on a task.");
+			getchar();
+			exit(1);
+		}
+	}
+
 	psSim->entry(psSim->pvParameters);
 
 	return 0;
 }
 
+void create_mmcss_registry_entry()
+{
+	HKEY hKey;
+	REGSAM flags = KEY_WRITE;
+	if(bIsWow64)
+		flags |= KEY_WOW64_64KEY;
+
+
+	if(RegCreateKeyEx(HKEY_LOCAL_MACHINE,
+			TEXT("SOFTWARE\\Microsoft\\Windows NT\\CurrentVersion\\Multimedia\\SystemProfile\\Tasks\\FreeRTOS"),
+			0, NULL, 0, flags, NULL,
+			&hKey, NULL) != ERROR_SUCCESS)
+	{
+		printf("Internal error creating MMCSS registry key.");
+		getchar();
+		exit(1);
+	}
+
+	DWORD affinity = 0;
+	DWORD clock_rate = 10000;
+	DWORD gpu_priority = 8;
+	DWORD priority = 2;
+
+	TCHAR *background_only = TEXT("True");
+	TCHAR *scheduling_category = TEXT("High");
+	TCHAR *sfio_priority = TEXT("Normal");
+
+	RegSetValueEx(hKey, TEXT("Affinity"), 0, REG_DWORD,
+			(BYTE *) &affinity, sizeof(DWORD));
+	RegSetValueEx(hKey, TEXT("Clock Rate"), 0, REG_DWORD,
+			(BYTE *) &clock_rate, sizeof(DWORD));
+	RegSetValueEx(hKey, TEXT("GPU Priority"), 0, REG_DWORD,
+			(BYTE *) &gpu_priority, sizeof(DWORD));
+	RegSetValueEx(hKey, TEXT("Priority"), 0, REG_DWORD,
+			(BYTE *) &priority, sizeof(DWORD));
+
+	RegSetValueEx(hKey, TEXT("Background Only"), 0, REG_SZ,
+			(BYTE *) background_only,
+			(DWORD)(lstrlen(background_only)+1)*sizeof(TCHAR));
+	RegSetValueEx(hKey, TEXT("Scheduling Category"), 0, REG_SZ,
+			(BYTE *) scheduling_category,
+			(DWORD)(lstrlen(scheduling_category)+1)*sizeof(TCHAR));
+	RegSetValueEx(hKey, TEXT("SFIO Priority"), 0, REG_SZ,
+			(BYTE *) sfio_priority,
+			(DWORD)(lstrlen(sfio_priority)+1)*sizeof(TCHAR));
+
+	RegCloseKey(hKey);
+}
+
+
+//Boldly taken from MSDN
+BOOL IsUserAdmin(VOID)
+/*++
+Routine Description: This routine returns TRUE if the caller's
+process is a member of the Administrators local group. Caller is NOT
+expected to be impersonating anyone and is expected to be able to
+open its own process and process token.
+Arguments: None.
+Return Value:
+   TRUE - Caller has Administrators local group.
+   FALSE - Caller does not have Administrators local group. --
+*/
+{
+	BOOL b;
+	SID_IDENTIFIER_AUTHORITY NtAuthority = {SECURITY_NT_AUTHORITY};
+	PSID AdministratorsGroup;
+	b = AllocateAndInitializeSid(
+	    &NtAuthority,
+        2,
+        SECURITY_BUILTIN_DOMAIN_RID,
+        DOMAIN_ALIAS_RID_ADMINS,
+        0, 0, 0, 0, 0, 0,
+        &AdministratorsGroup);
+	if(b)
+	{
+		if (!CheckTokenMembership( NULL, AdministratorsGroup, &b))
+		{
+			b = FALSE;
+		}
+		FreeSid(AdministratorsGroup);
+	}
+
+	return b;
+}
+
+//Also boldly taken from MSDN
+void CheckIsWow64()
+{
+	LPFN_ISWOW64PROCESS fnIsWow64Process;
+
+	bIsWow64 = FALSE;
+
+    //IsWow64Process is not available on all supported versions of Windows.
+    //Use GetModuleHandle to get a handle to the DLL that contains the function
+    //and GetProcAddress to get a pointer to the function if available.
+
+    fnIsWow64Process = (LPFN_ISWOW64PROCESS) GetProcAddress(
+        GetModuleHandle(TEXT("kernel32")),"IsWow64Process");
+
+    if(NULL != fnIsWow64Process)
+    {
+        if (!fnIsWow64Process(GetCurrentProcess(),&bIsWow64))
+        {
+            printf("Error determining if FreeRTOS is running in WOW64 mode.");
+            getchar();
+            exit(1);
+        }
+    }
+}
+
+//Check whether we're an elevated process under UAC (only works with Vista and 7)
+BOOL is_elevated_process()
+{
+	HANDLE tokenHandle;
+	TOKEN_ELEVATION isElevated;
+	DWORD returnLength;
+
+	if(!OpenProcessToken(GetCurrentProcess(), TOKEN_QUERY, &tokenHandle))
+	{
+		printf("Error opening process token while verifying elevated rights.");
+		getchar();
+		exit(1);
+	}
+
+	if(!GetTokenInformation(tokenHandle,
+			TokenElevation,
+			(void *) &isElevated, sizeof(isElevated), &returnLength))
+	{
+		printf("Error getting token information while verifying elevated rights.");
+		CloseHandle(tokenHandle);
+		getchar();
+		exit(1);
+	}
+
+	CloseHandle(tokenHandle);
+
+	return (isElevated.TokenIsElevated != 0);
+}
+
+void check_mmcss_registry_entry()
+{
+	HKEY hKey;
+	DWORD numSubKeys;
+	TCHAR subKeyName[MAX_KEY_LENGTH];
+	DWORD subKeyNameSize;
+	BOOL exists = FALSE;
+	REGSAM flags = KEY_READ;
+	LONG enumError;
+	if(bIsWow64)
+		flags |= KEY_WOW64_64KEY;
+
+	//Check for the registry entry
+
+	if(RegOpenKeyEx(HKEY_LOCAL_MACHINE,
+			TEXT("SOFTWARE\\Microsoft\\Windows NT\\CurrentVersion\\Multimedia\\SystemProfile\\Tasks"),
+			0, flags,
+			&hKey) != ERROR_SUCCESS)
+	{
+		printf("Error opening MMCSS registry key.");
+		getchar();
+		exit(1);
+	}
+
+	//Enumerate through all subkeys
+	RegQueryInfoKey(hKey, NULL, NULL, NULL,
+			&numSubKeys, NULL, NULL, NULL, NULL, NULL, NULL, NULL);
+	//printf("Num subkeys: %d\n", (int) numSubKeys);
+	for(DWORD i = 0; i < numSubKeys; i++)
+	{
+		subKeyNameSize = MAX_KEY_LENGTH;
+		if((enumError = RegEnumKeyEx(hKey, i, subKeyName, &subKeyNameSize,
+				NULL, NULL, NULL, NULL)) == ERROR_SUCCESS)
+		{
+			//printf("Subkey name: %s\n", subKeyName);
+			if(lstrcmp(subKeyName, TEXT("FreeRTOS")) == 0)
+			{
+				exists = TRUE;
+			}
+		}
+		else
+			printf("Error enumerating subkeys: %d\n", (int) enumError);
+	}
+
+	RegCloseKey(hKey);
+
+	if(exists)
+	{
+		return;
+	}
+
+	//it doesn't exist
+	//Create it
+
+	//Check for sufficient privileges
+	if(!is_elevated_process())
+	{
+		printf("Error: this program needs elevated privileges to run properly the first time only.\n");
+		printf("Please click 'run as administrator'.\n");
+		getchar();
+		exit(1);
+	}
+
+	create_mmcss_registry_entry();
+}
+
+void load_mmcss()
+{
+	HMODULE hAvrt = LoadLibrary(TEXT("Avrt.dll"));
+	if(hAvrt == NULL)
+	{
+		printf("Error: could not find avrt.dll.");
+		getchar();
+		exit(1);
+	}
+
+	AvSetMmThreadCharacteristics = (set_mm_thread_characteristics)
+			GetProcAddress(hAvrt, TEXT("AvSetMmThreadCharacteristicsA"));
+	AvSetMmThreadPriority = (set_mm_thread_priority)
+			GetProcAddress(hAvrt, TEXT("AvSetMmThreadPriority"));
+}
+
 static void create_system_objects(void)
 {
+	OSVERSIONINFO version;
+
+	version.dwOSVersionInfoSize = sizeof(version);
+	GetVersionEx(&version);
+
+	//For operating systems up to XP, increase the clock rate
+	//and increase the priority as much as possible
+	if(version.dwMajorVersion < 6)
+	{
+		if(IsUserAdmin())
+			SetPriorityClass(GetCurrentProcess(), REALTIME_PRIORITY_CLASS);
+		else
+			SetPriorityClass(GetCurrentProcess(), HIGH_PRIORITY_CLASS);
+
+		//Set timer
+
+		TIMECAPS tc;
+		timeGetDevCaps(&tc, sizeof(tc));
+		msPerTick = tc.wPeriodMin;
+		debug_printf("Ms per tick: %i\n", msPerTick);
+		if(msPerTick > 2)
+		{
+			printf("Warning: your system timer has a low resolution.\n");
+			printf("Either decrease the tick rate, or get a better PC!\n");
+		}
+		timeBeginPeriod(tc.wPeriodMin);
+		bUsingMMCSS = FALSE;
+	}
+	else
+	{
+		//From Vista onward, use MMCSS
+		CheckIsWow64();
+		check_mmcss_registry_entry();
+		load_mmcss();
+		bUsingMMCSS = TRUE;
+	}
+
 	DuplicateHandle(
 		GetCurrentProcess(),
 		GetCurrentThread(),
@@ -278,8 +585,20 @@ static void create_system_objects(void)
 		FALSE,
 		DUPLICATE_SAME_ACCESS);
 
-	SetThreadPriority(hIsrDispatcher, THREAD_PRIORITY_BELOW_NORMAL);
+	if(bUsingMMCSS)
-	SetPriorityClass(GetCurrentProcess(), HIGH_PRIORITY_CLASS);
+	{
+		DWORD taskIndex = 0;
+		HANDLE AvRtHandle = AvSetMmThreadCharacteristics("FreeRTOS", &taskIndex);
+		if(AvRtHandle == NULL)
+		{
+			printf("Error setting MMCSS on the scheduler.");
+			getchar();
+			exit(1);
+		}
+		AvSetMmThreadPriority(AvRtHandle, AVRT_PRIORITY_LOW);
+	}
+	else
+		SetThreadPriority(hIsrDispatcher, THREAD_PRIORITY_BELOW_NORMAL);
 
 	hIsrMutex = CreateMutex(NULL, FALSE, NULL);
 	hIsrInvoke = CreateEvent(NULL, FALSE, FALSE, NULL);
@@ -288,22 +607,12 @@ static void create_system_objects(void)
 
 	dwEnabledIsr |= (1<<CPU_INTR_TICK);
 
-	//Set timer
-
-	TIMECAPS tc;
-	timeGetDevCaps(&tc, sizeof(tc));
-	msPerTick = tc.wPeriodMin;
-	debug_printf("Ms per tick: %i\n", msPerTick);
-	if(msPerTick > 2)
-	{
-		printf("Warning: your system timer has a low resolution.\n");
-		printf("Either decrease the tick rate, or get a better PC!\n");
-	}
-	timeBeginPeriod(tc.wPeriodMin);
-
 #if configUSE_PREEMPTION != 0
-	SetThreadPriority(CreateThread(NULL, 0, tick_generator, NULL, 0, NULL),
+	if(bUsingMMCSS)
-		THREAD_PRIORITY_ABOVE_NORMAL);
+		CreateThread(NULL, 0, tick_generator, NULL, 0, NULL); //It uses MMCSS too
+	else
+		SetThreadPriority(CreateThread(NULL, 0, tick_generator, NULL, 0, NULL),
+				THREAD_PRIORITY_ABOVE_NORMAL);
 #endif
 }
 

flight/PiOS.win32/win32/Libraries/FreeRTOS/Source/portable/GCC/Win32/portmacro.h

@@ -53,6 +53,8 @@
 #ifndef PORTMACRO_H
 #define PORTMACRO_H
 
+#define _WIN32_WINNT 0x600
+
 #include <windows.h>
 #include <tchar.h>
 #include <stdio.h>

flight/PiOS.win32/win32/pios_servo.c

@@ -0,0 +1,74 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup   PIOS_SERVO RC Servo Functions
+ * @brief Code to do set RC servo output
+ * @{
+ *
+ * @file       pios_servo.c  
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      RC Servo routines (STM32 dependent)
+ * @see        The GNU Public License (GPL) Version 3
+ * 
+ *****************************************************************************/
+/* 
+ * This program is free software; you can redistribute it and/or modify 
+ * it under the terms of the GNU General Public License as published by 
+ * the Free Software Foundation; either version 3 of the License, or 
+ * (at your option) any later version.
+ * 
+ * This program 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 this program; if not, write to the Free Software Foundation, Inc., 
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/* Project Includes */
+#include "pios.h"
+
+#if defined(PIOS_INCLUDE_SERVO)
+
+/* Private Function Prototypes */
+
+/* Local Variables */
+static volatile uint16_t ServoPosition[PIOS_SERVO_NUM_TIMERS];
+
+/**
+* Initialise Servos
+*/
+void PIOS_Servo_Init(void)
+{
+}
+
+/**
+* Set the servo update rate (Max 500Hz)
+* \param[in] onetofour Rate for outputs 1 to 4 (Hz)
+* \param[in] fivetoeight Rate for outputs 5 to 8 (Hz)
+*/
+void PIOS_Servo_SetHz(uint16_t onetofour, uint16_t fivetoeight)
+{
+}
+
+/**
+* Set servo position
+* \param[in] Servo Servo number (0-7)
+* \param[in] Position Servo position in milliseconds
+*/
+void PIOS_Servo_Set(uint8_t Servo, uint16_t Position)
+{
+#ifndef PIOS_ENABLE_DEBUG_PINS
+	/* Make sure servo exists */
+	if (Servo < PIOS_SERVO_NUM_OUTPUTS && Servo >= 0) {
+		/* Update the position */
+		ServoPosition[Servo] = Position;
+
+	}
+#endif // PIOS_ENABLE_DEBUG_PINS
+}
+
+#endif