Skip to content

Commit

Permalink
Updated Windows compatibility of C++ sample
Browse files Browse the repository at this point in the history
  • Loading branch information
lebarsfa committed Dec 27, 2017
1 parent 8668656 commit 9850562
Show file tree
Hide file tree
Showing 7 changed files with 149 additions and 28 deletions.
18 changes: 9 additions & 9 deletions C++/RazorAHRS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -167,7 +167,7 @@ RazorAHRS::_init_razor()
else
{
if (errno != EAGAIN && errno != EINTR)
throw std::runtime_error("Can not read from serial port (1).");
throw std::runtime_error("Cannot read from serial port (1).");
}

// check timeout
Expand All @@ -181,7 +181,7 @@ RazorAHRS::_init_razor()
}
if (elapsed_ms(t0, t2) > _connect_timeout_ms)
// timeout -> tracker not present
throw std::runtime_error("Can not init: tracker does not answer.");
throw std::runtime_error("Cannot init: tracker does not answer.");
}


Expand All @@ -196,7 +196,7 @@ RazorAHRS::_init_razor()
if (_mode == YAW_PITCH_ROLL) config = "#ob" + config;
else if (_mode == ACC_MAG_GYR_RAW) config = "#osrb" + config;
else if (_mode == ACC_MAG_GYR_CALIBRATED) config = "#oscb" + config;
else throw std::runtime_error("Can not init: unknown 'mode' parameter.");
else throw std::runtime_error("Cannot init: unknown 'mode' parameter.");

write(_serial_port, config.data(), config.length());

Expand All @@ -219,7 +219,7 @@ RazorAHRS::_init_razor()
else
{
if (errno != EAGAIN && errno != EINTR)
throw std::runtime_error("Can not read from serial port (2).");
throw std::runtime_error("Cannot read from serial port (2).");
}
}

Expand Down Expand Up @@ -313,10 +313,10 @@ RazorAHRS::_thread(void *arg)
if (_mode == YAW_PITCH_ROLL) { // 3 floats
if (_input_pos == 12) // we received a full frame
{
// convert endianess if necessary
// convert endianness if necessary
if (_big_endian())
{
_swap_endianess(_input_buf.ypr, 3);
_swap_endianness(_input_buf.ypr, 3);
}

// invoke callback
Expand All @@ -327,10 +327,10 @@ RazorAHRS::_thread(void *arg)
} else { // raw or calibrated sensor data (9 floats)
if (_input_pos == 36) // we received a full frame
{
// convert endianess if necessary
// convert endianness if necessary
if (_big_endian())
{
_swap_endianess(_input_buf.amg, 9);
_swap_endianness(_input_buf.amg, 9);
}

// invoke callback
Expand All @@ -345,7 +345,7 @@ RazorAHRS::_thread(void *arg)
{
if (errno != EAGAIN && errno != EINTR)
{
error("Can not read from serial port (3).");
error("Cannot read from serial port (3).");
return arg;
}
}
Expand Down
22 changes: 11 additions & 11 deletions C++/RazorAHRS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,7 @@ class RazorAHRS
/* threading stuff */
pthread_t _thread_id;
void* _thread(void*); // thread main function
volatile bool _stop_thread; // thred stop flag
volatile bool _stop_thread; // thread stop flag

// start the tracking thread
void _start_io_thread()
Expand Down Expand Up @@ -124,14 +124,14 @@ class RazorAHRS
return (*(reinterpret_cast<const char*> (&num))) != 1;
}

// swap endianess of int
void _swap_endianess(int &i)
// swap endianness of int
void _swap_endianness(int &i)
{
i = (i >> 24) | ((i << 8) & 0x00FF0000) | ((i >> 8) & 0x0000FF00) | (i << 24);
}

// swap endianess of float
void _swap_endianess(float &f)
// swap endianness of float
void _swap_endianness(float &f)
{
float swapped;
char *f_as_char = reinterpret_cast<char*> (&f);
Expand All @@ -146,18 +146,18 @@ class RazorAHRS
f = swapped;
}

// swap endianess of int array
void _swap_endianess(int arr[], int arr_length)
// swap endianness of int array
void _swap_endianness(int arr[], int arr_length)
{
for (int i = 0; i < arr_length; i++)
_swap_endianess(arr[i]);
_swap_endianness(arr[i]);
}

// swap endianess of float array
void _swap_endianess(float arr[], int arr_length)
// swap endianness of float array
void _swap_endianness(float arr[], int arr_length)
{
for (int i = 0; i < arr_length; i++)
_swap_endianess(arr[i]);
_swap_endianness(arr[i]);
}
};

Expand Down
2 changes: 1 addition & 1 deletion C++/RazorAHRS.pro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@ CONFIG -= qt
DEFINES -= UNICODE

win32:INCLUDEPATH += unix_adapt
win32:DEFINES += DISABLE_TIMEZONE_STRUCT_REDEFINITION
win32:DEFINES += DISABLE_TIMEZONE_STRUCT_REDEFINITION ENABLE_O_NDELAY_WORKAROUND

DEFINES += _REENTRANT
LIBS += -lpthread
Expand Down
8 changes: 4 additions & 4 deletions C++/RazorAHRS.vcxproj
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,7 +105,7 @@
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
<ClCompile>
<Optimization>Disabled</Optimization>
<PreprocessorDefinitions>WIN32;HAVE_STRUCT_TIMESPEC;_REENTRANT;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<PreprocessorDefinitions>HAVE_STRUCT_TIMESPEC;ENABLE_O_NDELAY_WORKAROUND;_REENTRANT;WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<MinimalRebuild>true</MinimalRebuild>
<BasicRuntimeChecks>EnableFastChecks</BasicRuntimeChecks>
<RuntimeLibrary>MultiThreadedDebugDLL</RuntimeLibrary>
Expand All @@ -129,7 +129,7 @@
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
<ClCompile>
<Optimization>Disabled</Optimization>
<PreprocessorDefinitions>WIN32;HAVE_STRUCT_TIMESPEC;_REENTRANT;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<PreprocessorDefinitions>HAVE_STRUCT_TIMESPEC;ENABLE_O_NDELAY_WORKAROUND;_REENTRANT;WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<BasicRuntimeChecks>EnableFastChecks</BasicRuntimeChecks>
<RuntimeLibrary>MultiThreadedDebugDLL</RuntimeLibrary>
<PrecompiledHeader>
Expand All @@ -153,7 +153,7 @@
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
<ClCompile>
<PreprocessorDefinitions>WIN32;HAVE_STRUCT_TIMESPEC;_REENTRANT;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<PreprocessorDefinitions>HAVE_STRUCT_TIMESPEC;ENABLE_O_NDELAY_WORKAROUND;_REENTRANT;WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<RuntimeLibrary>MultiThreaded</RuntimeLibrary>
<PrecompiledHeader />
<WarningLevel>Level4</WarningLevel>
Expand All @@ -175,7 +175,7 @@
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
<ClCompile>
<PreprocessorDefinitions>WIN32;HAVE_STRUCT_TIMESPEC;_REENTRANT;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<PreprocessorDefinitions>HAVE_STRUCT_TIMESPEC;ENABLE_O_NDELAY_WORKAROUND;_REENTRANT;WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<RuntimeLibrary>MultiThreaded</RuntimeLibrary>
<PrecompiledHeader>
</PrecompiledHeader>
Expand Down
74 changes: 74 additions & 0 deletions C++/unix_adapt/bits/fcntl2.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,6 +81,11 @@ __inline int fcntl(int __fd, int __cmd, ...)
{
HANDLE hDev = (HANDLE)(intptr_t)__fd;
COMMTIMEOUTS timeouts;
// Disable if XON/XOFF are necessary, enable if fcntl() is used together with O_NDELAY and timeouts...
#ifdef ENABLE_O_NDELAY_WORKAROUND
DCB dcb;
DWORD ReadIntervalTimeout = 0, ReadTotalTimeoutConstant = 0, ReadTotalTimeoutMultiplier = 0, WriteTotalTimeoutConstant = 0;
#endif // ENABLE_O_NDELAY_WORKAROUND
va_list argptr;
va_start(argptr, __cmd);

Expand All @@ -96,6 +101,41 @@ __inline int fcntl(int __fd, int __cmd, ...)
}
if (flags & O_NDELAY)
{
// Disable if XON/XOFF are necessary, enable if fcntl() is used together with O_NDELAY and timeouts...
#ifdef ENABLE_O_NDELAY_WORKAROUND
// Need to save the original timeouts settings for next calls of fcntl()...

memset(&dcb, 0, sizeof(DCB));
if (!GetCommState(hDev, &dcb))
{
errno = EIO;
return -1;
}

// Warning : XoffLim and XonLim are just used as placeholders to save the original timeouts settings,
// they are not normally related to timeouts...

ReadIntervalTimeout = timeouts.ReadIntervalTimeout;
ReadTotalTimeoutConstant = timeouts.ReadTotalTimeoutConstant;
ReadTotalTimeoutMultiplier = timeouts.ReadTotalTimeoutMultiplier;
WriteTotalTimeoutConstant = timeouts.WriteTotalTimeoutConstant;
if (ReadIntervalTimeout == MAXDWORD) ReadIntervalTimeout = 255*100;
else if (ReadIntervalTimeout/100 > 254) ReadIntervalTimeout = 254*100;
if (ReadTotalTimeoutConstant == MAXDWORD) ReadTotalTimeoutConstant = 255*100;
else if (ReadTotalTimeoutConstant/100 > 254) ReadTotalTimeoutConstant = 254*100;
if (ReadTotalTimeoutMultiplier == MAXDWORD) ReadTotalTimeoutMultiplier = 255*100;
else if (ReadTotalTimeoutMultiplier/100 > 254) ReadTotalTimeoutMultiplier = 254*100;
if (WriteTotalTimeoutConstant == MAXDWORD) WriteTotalTimeoutConstant = 255*100;
else if (WriteTotalTimeoutConstant/100 > 254) WriteTotalTimeoutConstant = 254*100;
dcb.XoffLim = (WORD)(((ReadIntervalTimeout/100)<<8)|(ReadTotalTimeoutConstant/100));
dcb.XonLim = (WORD)(((ReadTotalTimeoutMultiplier/100)<<8)|(WriteTotalTimeoutConstant/100));
if (!SetCommState(hDev, &dcb))
{
errno = EIO;
return -1;
}
#endif // ENABLE_O_NDELAY_WORKAROUND
// Change timeout settings.
timeouts.ReadIntervalTimeout = MAXDWORD;
timeouts.ReadTotalTimeoutConstant = 0;
timeouts.ReadTotalTimeoutMultiplier = 0;
Expand All @@ -109,11 +149,45 @@ __inline int fcntl(int __fd, int __cmd, ...)
}
else
{
// Disable if XON/XOFF are necessary, enable if fcntl() is used together with O_NDELAY and timeouts...
#ifdef ENABLE_O_NDELAY_WORKAROUND
// Should restore original timeout values...

memset(&dcb, 0, sizeof(DCB));
if (!GetCommState(hDev, &dcb))
{
errno = EIO;
return -1;
}

// Warning : XoffLim and XonLim are just used as placeholders to save the original timeouts settings,
// they are not normally related to timeouts...

if ((dcb.XoffLim>>8) == 255) timeouts.ReadIntervalTimeout = MAXDWORD;
else timeouts.ReadIntervalTimeout = (dcb.XoffLim>>8)*100;
if ((dcb.XoffLim&0x00FF) == 255) timeouts.ReadTotalTimeoutConstant = MAXDWORD;
else timeouts.ReadTotalTimeoutConstant = (dcb.XoffLim&0x00FF)*100;
if ((dcb.XonLim>>8) == 255) timeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
else timeouts.ReadTotalTimeoutMultiplier = (dcb.XonLim>>8)*100;
if ((dcb.XonLim&0x00FF) == 255) timeouts.WriteTotalTimeoutConstant = MAXDWORD;
else timeouts.WriteTotalTimeoutConstant = (dcb.XonLim&0x00FF)*100;
timeouts.WriteTotalTimeoutMultiplier = 0;
// Set to full blocking in case the restored values were non-blocking.
if ((timeouts.ReadIntervalTimeout == MAXDWORD)&&(timeouts.ReadTotalTimeoutConstant == 0)&&(timeouts.ReadTotalTimeoutMultiplier == 0))
{
timeouts.ReadIntervalTimeout = 0;
timeouts.ReadTotalTimeoutConstant = 0;
timeouts.ReadTotalTimeoutMultiplier = 0;
timeouts.WriteTotalTimeoutConstant = 0;
timeouts.WriteTotalTimeoutMultiplier = 0;
}
#else
timeouts.ReadIntervalTimeout = 0;
timeouts.ReadTotalTimeoutConstant = 0;
timeouts.ReadTotalTimeoutMultiplier = 0;
timeouts.WriteTotalTimeoutConstant = 0;
timeouts.WriteTotalTimeoutMultiplier = 0;
#endif // ENABLE_O_NDELAY_WORKAROUND
if (!SetCommTimeouts(hDev, &timeouts))
{
errno = EIO;
Expand Down
50 changes: 48 additions & 2 deletions C++/unix_adapt/termios.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -135,7 +135,6 @@ __inline int tcgetattr(int __fd, struct termios *__termios_p)

//#pragma region TIMEOUTS
memset(&timeouts, 0, sizeof(COMMTIMEOUTS));

if (!GetCommTimeouts(hDev, &timeouts))
{
errno = EIO;
Expand Down Expand Up @@ -195,7 +194,6 @@ __inline int tcgetattr(int __fd, struct termios *__termios_p)

//#pragma region DCB
memset(&dcb, 0, sizeof(DCB));

if (!GetCommState(hDev, &dcb))
{
errno = EIO;
Expand Down Expand Up @@ -381,12 +379,20 @@ __inline int tcsetattr(int __fd, int __optional_actions, const struct termios *_
{
HANDLE hDev = (HANDLE)(intptr_t)__fd;
COMMTIMEOUTS timeouts;
#ifdef ENABLE_O_NDELAY_WORKAROUND
DWORD ReadIntervalTimeout = 0, ReadTotalTimeoutConstant = 0, ReadTotalTimeoutMultiplier = 0, WriteTotalTimeoutConstant = 0;
#endif // ENABLE_O_NDELAY_WORKAROUND
DCB dcb;

UNREFERENCED_PARAMETER(__optional_actions);

//#pragma region TIMEOUTS
memset(&timeouts, 0, sizeof(COMMTIMEOUTS));
if (!GetCommTimeouts(hDev, &timeouts))
{
errno = EIO;
return -1;
}

if ((__termios_p->c_cc[VMIN] == 0)&&(__termios_p->c_cc[VTIME]) == 0)
{
Expand Down Expand Up @@ -438,6 +444,41 @@ __inline int tcsetattr(int __fd, int __optional_actions, const struct termios *_
timeouts.WriteTotalTimeoutConstant = 0; // ???
}

// Disable if XON/XOFF are necessary, enable if fcntl() is used together with O_NDELAY and timeouts...
#ifdef ENABLE_O_NDELAY_WORKAROUND
// Need to save the original timeouts settings for next calls of fcntl()...

memset(&dcb, 0, sizeof(DCB));
if (!GetCommState(hDev, &dcb))
{
errno = EIO;
return -1;
}

// Warning : XoffLim and XonLim are just used as placeholders to save the original timeouts settings,
// they are not normally related to timeouts...

ReadIntervalTimeout = timeouts.ReadIntervalTimeout;
ReadTotalTimeoutConstant = timeouts.ReadTotalTimeoutConstant;
ReadTotalTimeoutMultiplier = timeouts.ReadTotalTimeoutMultiplier;
WriteTotalTimeoutConstant = timeouts.WriteTotalTimeoutConstant;
if (ReadIntervalTimeout == MAXDWORD) ReadIntervalTimeout = 255*100;
else if (ReadIntervalTimeout/100 > 254) ReadIntervalTimeout = 254*100;
if (ReadTotalTimeoutConstant == MAXDWORD) ReadTotalTimeoutConstant = 255*100;
else if (ReadTotalTimeoutConstant/100 > 254) ReadTotalTimeoutConstant = 254*100;
if (ReadTotalTimeoutMultiplier == MAXDWORD) ReadTotalTimeoutMultiplier = 255*100;
else if (ReadTotalTimeoutMultiplier/100 > 254) ReadTotalTimeoutMultiplier = 254*100;
if (WriteTotalTimeoutConstant == MAXDWORD) WriteTotalTimeoutConstant = 255*100;
else if (WriteTotalTimeoutConstant/100 > 254) WriteTotalTimeoutConstant = 254*100;
dcb.XoffLim = (WORD)(((ReadIntervalTimeout/100)<<8)|(ReadTotalTimeoutConstant/100));
dcb.XonLim = (WORD)(((ReadTotalTimeoutMultiplier/100)<<8)|(WriteTotalTimeoutConstant/100));
if (!SetCommState(hDev, &dcb))
{
errno = EIO;
return -1;
}
#endif // ENABLE_O_NDELAY_WORKAROUND

if (!SetCommTimeouts(hDev, &timeouts))
{
errno = EIO;
Expand All @@ -447,6 +488,11 @@ __inline int tcsetattr(int __fd, int __optional_actions, const struct termios *_

//#pragma region DCB
memset(&dcb, 0, sizeof(DCB));
if (!GetCommState(hDev, &dcb))
{
errno = EIO;
return -1;
}

// Not possible to have different c_ispeed and c_ospeed on Windows...
dcb.BaudRate = _linuxbaudrate2windows(__termios_p->c_ispeed);
Expand Down
3 changes: 2 additions & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@ Tutorial
---

You find a [detailed tutorial in the Wiki](https://github.com/ptrbrtz/razor-9dof-ahrs/wiki/Tutorial).

Note: For SEN-14001 (*9DoF Razor IMU M0*), you will need to follow the same instructions as for the default firmware on https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide and use an updated version of SparkFun_MPU-9250-DMP_Arduino_Library from https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library (an updated version of the default firmware is also available on https://github.com/lebarsfa/9DOF_Razor_IMU).

Quick setup
Expand All @@ -27,7 +28,7 @@ Run `Processing/Razor_AHRS_test/Razor_AHRS_test.pde` using *Processing*.

### Optional: Mac OS X / Unix / Linux / Windows C++ Interface

Use the provided Qt project (check Projects\Run Settings\Run in terminal to force your application to run inside a separate terminal) or compile test program from the command line (add `-Iunix_adapt -DDISABLE_TIMEZONE_STRUCT_REDEFINITION` for MinGW/MSYS):
Use the provided Qt project (check Projects\Run Settings\Run in terminal to force your application to run inside a separate terminal) or compile test program from the command line (add `-Iunix_adapt -DDISABLE_TIMEZONE_STRUCT_REDEFINITION -DENABLE_O_NDELAY_WORKAROUND` for MinGW/MSYS):

g++ Example.cpp RazorAHRS.cpp -Wall -D_REENTRANT -lpthread -o example

Expand Down

0 comments on commit 9850562

Please sign in to comment.