rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

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.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2270 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
cwabbott 2010-12-22 14:47:42 +00:00 committed by cwabbott
parent c6f14c7530
commit 7715a6d6cd
7 changed files with 453 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
flight/OpenPilot/Makefile.win32
View File

@ -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

3
flight/PiOS.win32/inc/pios_posix.h
View File

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

43
flight/PiOS.win32/inc/pios_servo.h Normal file
View File

@ -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 */
/**
* @}
* @}
*/

3
flight/PiOS.win32/pios.h
View File

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

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

@ -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);
SetPriorityClass(GetCurrentProcess(), HIGH_PRIORITY_CLASS);
if(bUsingMMCSS)
{
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),
THREAD_PRIORITY_ABOVE_NORMAL);
if(bUsingMMCSS)
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
}

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

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

74
flight/PiOS.win32/win32/pios_servo.c Normal file
View File

@ -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