Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 04:06:33 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

drivers/inv/vectornav: fix official vectornav library NuttX support

Browse Source 
- vectornav library (libvnc) fixed for NuttX
   - open serial port O_NONBLOCK (like __APPLE__)
   - set serial port baud rate with cfsetspeed (like __APPLE__)
 - vectornav backend thread increase stack and run at higher priority (SCHED_FIFO)
This commit is contained in:
Daniel Agar
2023-02-24 16:59:38 -05:00
committed by GitHub
parent 837095b9a8
commit a06a635da3
11 changed files with 81 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
+12 -12
View File
@@ -76,18 +76,18 @@ static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18};
static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19}; static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19};
static constexpr wq_config_t ttyS0{"wq:ttyS0", 1632, -21}; static constexpr wq_config_t ttyS0{"wq:ttyS0", 1728, -21};
static constexpr wq_config_t ttyS1{"wq:ttyS1", 1632, -22}; static constexpr wq_config_t ttyS1{"wq:ttyS1", 1728, -22};
static constexpr wq_config_t ttyS2{"wq:ttyS2", 1632, -23}; static constexpr wq_config_t ttyS2{"wq:ttyS2", 1728, -23};
static constexpr wq_config_t ttyS3{"wq:ttyS3", 1632, -24}; static constexpr wq_config_t ttyS3{"wq:ttyS3", 1728, -24};
static constexpr wq_config_t ttyS4{"wq:ttyS4", 1632, -25}; static constexpr wq_config_t ttyS4{"wq:ttyS4", 1728, -25};
static constexpr wq_config_t ttyS5{"wq:ttyS5", 1632, -26}; static constexpr wq_config_t ttyS5{"wq:ttyS5", 1728, -26};
static constexpr wq_config_t ttyS6{"wq:ttyS6", 1632, -27}; static constexpr wq_config_t ttyS6{"wq:ttyS6", 1728, -27};
static constexpr wq_config_t ttyS7{"wq:ttyS7", 1632, -28}; static constexpr wq_config_t ttyS7{"wq:ttyS7", 1728, -28};
static constexpr wq_config_t ttyS8{"wq:ttyS8", 1632, -29}; static constexpr wq_config_t ttyS8{"wq:ttyS8", 1728, -29};
static constexpr wq_config_t ttyS9{"wq:ttyS9", 1632, -30}; static constexpr wq_config_t ttyS9{"wq:ttyS9", 1728, -30};
static constexpr wq_config_t ttyACM0{"wq:ttyACM0", 1632, -31}; static constexpr wq_config_t ttyACM0{"wq:ttyACM0", 1728, -31};
static constexpr wq_config_t ttyUnknown{"wq:ttyUnknown", 1632, -32}; static constexpr wq_config_t ttyUnknown{"wq:ttyUnknown", 1728, -32};
static constexpr wq_config_t lp_default{"wq:lp_default", 1920, -50}; static constexpr wq_config_t lp_default{"wq:lp_default", 1920, -50};
src/drivers/ins/vectornav/VectorNav.cpp
+24 -11
View File
@@ -115,7 +115,8 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
GPSGROUP_NONE) GPSGROUP_NONE)
) { ) {
// TIMEGROUP_TIMESTARTUP // TIMEGROUP_TIMESTARTUP
//uint64_t time_startup = VnUartPacket_extractUint64(packet); uint64_t time_startup = VnUartPacket_extractUint64(packet);
(void)time_startup;
// IMUGROUP_UNCOMPACCEL // IMUGROUP_UNCOMPACCEL
vec3f accel = VnUartPacket_extractVec3f(packet); vec3f accel = VnUartPacket_extractVec3f(packet);
@@ -141,7 +142,8 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
GPSGROUP_NONE) GPSGROUP_NONE)
) { ) {
// TIMEGROUP_TIMESTARTUP // TIMEGROUP_TIMESTARTUP
//const uint64_t time_startup = VnUartPacket_extractUint64(packet); const uint64_t time_startup = VnUartPacket_extractUint64(packet);
(void)time_startup;
// IMUGROUP_TEMP // IMUGROUP_TEMP
const float temperature = VnUartPacket_extractFloat(packet); const float temperature = VnUartPacket_extractFloat(packet);
@@ -276,7 +278,8 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
GPSGROUP_NONE) GPSGROUP_NONE)
) { ) {
// TIMEGROUP_TIMESTARTUP // TIMEGROUP_TIMESTARTUP
//const uint64_t time_startup = VnUartPacket_extractUint64(packet); const uint64_t time_startup = VnUartPacket_extractUint64(packet);
(void)time_startup;
// GPSGROUP_UTC // GPSGROUP_UTC
// TimeUtc timeUtc; // TimeUtc timeUtc;
@@ -358,6 +361,8 @@ bool VectorNav::init()
if ((VnSensor_connect(&_vs, _port, DEFAULT_BAUDRATE) != E_NONE) || !VnSensor_verifySensorConnectivity(&_vs)) { if ((VnSensor_connect(&_vs, _port, DEFAULT_BAUDRATE) != E_NONE) || !VnSensor_verifySensorConnectivity(&_vs)) {
VnSensor_disconnect(&_vs);
static constexpr uint32_t BAUDRATES[] {9600, 19200, 38400, 57600, 115200, 128000, 230400, 460800, 921600}; static constexpr uint32_t BAUDRATES[] {9600, 19200, 38400, 57600, 115200, 128000, 230400, 460800, 921600};
for (auto &baudrate : BAUDRATES) { for (auto &baudrate : BAUDRATES) {
@@ -373,11 +378,19 @@ bool VectorNav::init()
} }
} }
if (!VnSensor_verifySensorConnectivity(&_vs)) {
PX4_ERR("Error verifying sensor connectivity");
VnSensor_disconnect(&_vs);
return false;
}
VnError error = E_NONE; VnError error = E_NONE;
// change baudrate to max // change baudrate to max
if ((error = VnSensor_changeBaudrate(&_vs, DESIRED_BAUDRATE)) != E_NONE) { if ((error = VnSensor_changeBaudrate(&_vs, DESIRED_BAUDRATE)) != E_NONE) {
PX4_WARN("Error changing baud rate failed: %d", error); PX4_ERR("Error changing baud rate failed: %d", error);
VnSensor_disconnect(&_vs);
return false;
} }
// query the sensor's model number // query the sensor's model number
@@ -385,6 +398,7 @@ bool VectorNav::init()
if ((error = VnSensor_readModelNumber(&_vs, model_number, sizeof(model_number))) != E_NONE) { if ((error = VnSensor_readModelNumber(&_vs, model_number, sizeof(model_number))) != E_NONE) {
PX4_ERR("Error reading model number %d", error); PX4_ERR("Error reading model number %d", error);
VnSensor_disconnect(&_vs);
return false; return false;
} }
@@ -393,6 +407,7 @@ bool VectorNav::init()
if ((error = VnSensor_readHardwareRevision(&_vs, &hardware_revision)) != E_NONE) { if ((error = VnSensor_readHardwareRevision(&_vs, &hardware_revision)) != E_NONE) {
PX4_ERR("Error reading HW revision %d", error); PX4_ERR("Error reading HW revision %d", error);
VnSensor_disconnect(&_vs);
return false; return false;
} }
@@ -401,6 +416,7 @@ bool VectorNav::init()
if ((error = VnSensor_readSerialNumber(&_vs, &serial_number)) != E_NONE) { if ((error = VnSensor_readSerialNumber(&_vs, &serial_number)) != E_NONE) {
PX4_ERR("Error reading serial number %d", error); PX4_ERR("Error reading serial number %d", error);
VnSensor_disconnect(&_vs);
return false; return false;
} }
@@ -409,6 +425,7 @@ bool VectorNav::init()
if ((error = VnSensor_readFirmwareVersion(&_vs, firmware_version, sizeof(firmware_version))) != E_NONE) { if ((error = VnSensor_readFirmwareVersion(&_vs, firmware_version, sizeof(firmware_version))) != E_NONE) {
PX4_ERR("Error reading firmware version %d", error); PX4_ERR("Error reading firmware version %d", error);
VnSensor_disconnect(&_vs);
return false; return false;
} }
@@ -456,7 +473,6 @@ bool VectorNav::configure()
PX4_ERR("Error reading VPE basic control %d", error); PX4_ERR("Error reading VPE basic control %d", error);
} }
VnError VnSensor_readImuFilteringConfiguration(VnSensor * sensor, ImuFilteringConfigurationRegister * fields); VnError VnSensor_readImuFilteringConfiguration(VnSensor * sensor, ImuFilteringConfigurationRegister * fields);
// VnError VnSensor_writeImuFilteringConfiguration(VnSensor *sensor, ImuFilteringConfigurationRegister fields, bool waitForReply); // VnError VnSensor_writeImuFilteringConfiguration(VnSensor *sensor, ImuFilteringConfigurationRegister fields, bool waitForReply);
@@ -479,7 +495,7 @@ bool VectorNav::configure()
// binary output 1: max rate IMU // binary output 1: max rate IMU
BinaryOutputRegister_initialize( BinaryOutputRegister_initialize(
&_binary_output_group_1, &_binary_output_group_1,
ASYNCMODE_PORT1, ASYNCMODE_PORT2,
1, // divider 1, // divider
COMMONGROUP_NONE, COMMONGROUP_NONE,
TIMEGROUP_TIMESTARTUP, TIMEGROUP_TIMESTARTUP,
@@ -502,7 +518,7 @@ bool VectorNav::configure()
// binary output 2: medium rate AHRS, INS, baro, mag // binary output 2: medium rate AHRS, INS, baro, mag
BinaryOutputRegister_initialize( BinaryOutputRegister_initialize(
&_binary_output_group_2, &_binary_output_group_2,
ASYNCMODE_PORT1, ASYNCMODE_PORT2,
8, // divider 8, // divider
COMMONGROUP_NONE, COMMONGROUP_NONE,
TIMEGROUP_TIMESTARTUP, TIMEGROUP_TIMESTARTUP,
@@ -521,7 +537,7 @@ bool VectorNav::configure()
// binary output 3: low rate GNSS // binary output 3: low rate GNSS
BinaryOutputRegister_initialize( BinaryOutputRegister_initialize(
&_binary_output_group_3, &_binary_output_group_3,
ASYNCMODE_PORT1, ASYNCMODE_PORT2,
80, // divider 80, // divider
COMMONGROUP_NONE, COMMONGROUP_NONE,
TIMEGROUP_TIMESTARTUP, TIMEGROUP_TIMESTARTUP,
@@ -577,9 +593,6 @@ void VectorNav::Run()
} }
} }
ScheduleDelayed(100_ms); ScheduleDelayed(100_ms);
} }
src/drivers/ins/vectornav/libvnc/include/vn/enum.h
+1 -1
View File
@@ -667,4 +667,4 @@ typedef enum
VNPROCESSOR_IMU /**< IMU Processor. */ VNPROCESSOR_IMU /**< IMU Processor. */
} VnProcessorType; } VnProcessorType;
#endif #endif
src/drivers/ins/vectornav/libvnc/include/vn/xplat/serialport.h
+2 -2
View File
@@ -26,9 +26,9 @@
#endif #endif
#endif #endif
#ifdef __linux__ #if defined(__linux__)
#include <linux/serial.h> #include <linux/serial.h>
#elif defined __APPLE__ #elif defined __APPLE__ || defined(__NUTTX__)
#include <dirent.h> #include <dirent.h>
#endif #endif
src/drivers/ins/vectornav/libvnc/include/vn/xplat/time.h
+1 -1
View File
@@ -39,7 +39,7 @@ typedef struct
#if _WIN32 #if _WIN32
double pcFrequency; double pcFrequency;
__int64 counterStart; __int64 counterStart;
#elif __linux__ || __APPLE__ ||__CYGWIN__ || __QNXNTO__ || defined __NUTTX__ #elif __linux__ || __APPLE__ ||__CYGWIN__ || __QNXNTO__ || __NUTTX__
double clockStart; double clockStart;
#else #else
#error "Unknown System" #error "Unknown System"
src/drivers/ins/vectornav/libvnc/src/vn/sensors/searcher.c
+4 -4
View File
@@ -9,7 +9,7 @@
#include <stdlib.h> #include <stdlib.h>
#ifdef __linux__ #if defined(__linux__) || defined(__NUTTX__)
#include <errno.h> #include <errno.h>
#include <fcntl.h> #include <fcntl.h>
#include <unistd.h> #include <unistd.h>
@@ -310,7 +310,7 @@ int32_t VnSearcher_getPortBaud(VnPortInfo* portInfo)
return portInfo->baud; return portInfo->baud;
} }
#ifdef __linux__ #if defined(__linux__) || defined(__NUTTX__)
void VnSearcher_findPorts_LINUX(char*** portNamesOut, int32_t* numPortsFound) void VnSearcher_findPorts_LINUX(char*** portNamesOut, int32_t* numPortsFound)
{ {
char portName[15] = {0}; char portName[15] = {0};
@@ -330,9 +330,9 @@ void VnSearcher_findPorts_LINUX(char*** portNamesOut, int32_t* numPortsFound)
/* Attempt to open the serial port */ /* Attempt to open the serial port */
portFd = open(portName, portFd = open(portName,
#if __linux__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__ #if __linux__ || __CYGWIN__ || __QNXNTO__
O_RDWR | O_NOCTTY ); O_RDWR | O_NOCTTY );
#elif __APPLE__ #elif __APPLE__ || __NUTTX__
O_RDWR | O_NOCTTY | O_NONBLOCK); O_RDWR | O_NOCTTY | O_NONBLOCK);
#else #else
#error "Unknown System" #error "Unknown System"
src/drivers/ins/vectornav/libvnc/src/vn/xplat/criticalsection.c
+4 -4
View File
@@ -6,7 +6,7 @@ VnError VnCriticalSection_initialize(VnCriticalSection *cs)
InitializeCriticalSection(&cs->handle); InitializeCriticalSection(&cs->handle);
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__ #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
if (pthread_mutex_init(&cs->handle, NULL)) if (pthread_mutex_init(&cs->handle, NULL))
return E_UNKNOWN; return E_UNKNOWN;
@@ -24,7 +24,7 @@ VnError VnCriticalSection_deinitialize(VnCriticalSection *cs)
DeleteCriticalSection(&cs->handle); DeleteCriticalSection(&cs->handle);
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__ #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
if (pthread_mutex_destroy(&cs->handle)) if (pthread_mutex_destroy(&cs->handle))
return E_UNKNOWN; return E_UNKNOWN;
@@ -42,7 +42,7 @@ VnError VnCriticalSection_enter(VnCriticalSection *cs)
EnterCriticalSection(&cs->handle); EnterCriticalSection(&cs->handle);
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__ #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
if (pthread_mutex_lock(&cs->handle)) if (pthread_mutex_lock(&cs->handle))
return E_UNKNOWN; return E_UNKNOWN;
@@ -60,7 +60,7 @@ VnError VnCriticalSection_leave(VnCriticalSection *cs)
LeaveCriticalSection(&cs->handle); LeaveCriticalSection(&cs->handle);
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__ #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
if (pthread_mutex_unlock(&cs->handle)) if (pthread_mutex_unlock(&cs->handle))
return E_UNKNOWN; return E_UNKNOWN;
src/drivers/ins/vectornav/libvnc/src/vn/xplat/event.c
+3 -3
View File
@@ -1,5 +1,5 @@
/* Enable IEEE Std 1003.1b-1993 functionality required for clock_gettime. */ /* Enable IEEE Std 1003.1b-1993 functionality required for clock_gettime. */
#ifdef __linux__ #if defined(__linux__) || defined(__NUTTX__)
/* Works for Ubuntu 15.10 */ /* Works for Ubuntu 15.10 */
#define _POSIX_C_SOURCE 199309L #define _POSIX_C_SOURCE 199309L
#elif defined __CYGWIN__ #elif defined __CYGWIN__
@@ -100,7 +100,7 @@ VnError VnEvent_waitMs(VnEvent *e, uint32_t timeoutMs)
return E_UNKNOWN; return E_UNKNOWN;
#elif defined __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__ #elif defined __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
return VnEvent_waitUs(e, timeoutMs * 1000); return VnEvent_waitUs(e, timeoutMs * 1000);
@@ -218,7 +218,7 @@ VnError VnEvent_signal(VnEvent *e)
if (!SetEvent(e->handle)) if (!SetEvent(e->handle))
return E_UNKNOWN; return E_UNKNOWN;
#elif defined __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__ #elif defined __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
if (pthread_mutex_lock(&e->mutex)) if (pthread_mutex_lock(&e->mutex))
return E_UNKNOWN; return E_UNKNOWN;
src/drivers/ins/vectornav/libvnc/src/vn/xplat/serialport.c
+15 -15
View File
@@ -2,7 +2,7 @@
#if _WIN32 #if _WIN32
/* Nothing to do. */ /* Nothing to do. */
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__ #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
#include <fcntl.h> #include <fcntl.h>
#include <errno.h> #include <errno.h>
#include <termios.h> #include <termios.h>
@@ -31,7 +31,7 @@ VnError VnSerialPort_initialize(VnSerialPort *sp)
#if _WIN32 #if _WIN32
sp->handle = NULL; sp->handle = NULL;
VnCriticalSection_initialize(&sp->readWriteCS); VnCriticalSection_initialize(&sp->readWriteCS);
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__ #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
sp->handle = -1; sp->handle = -1;
#else #else
#error "Unknown System" #error "Unknown System"
@@ -196,9 +196,9 @@ VnError VnSerialPort_open_internal(VnSerialPort *serialport, char const *portNam
portFd = open( portFd = open(
portName, portName,
#if __linux__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__ #if __linux__ || __CYGWIN__ || __QNXNTO__
O_RDWR | O_NOCTTY); O_RDWR | O_NOCTTY);
#elif __APPLE__ #elif __APPLE__ || __NUTTX__
O_RDWR | O_NOCTTY | O_NONBLOCK); O_RDWR | O_NOCTTY | O_NONBLOCK);
#else #else
#error "Unknown System" #error "Unknown System"
@@ -268,9 +268,9 @@ VnError VnSerialPort_open_internal(VnSerialPort *serialport, char const *portNam
} }
/* Set baudrate, 8n1, no modem control, enable receiving characters. */ /* Set baudrate, 8n1, no modem control, enable receiving characters. */
#if __linux__ || __QNXNTO__ || __CYGWIN__ || defined __NUTTX__ #if __linux__ || __QNXNTO__ || __CYGWIN__
portSettings.c_cflag = baudrateFlag; portSettings.c_cflag = baudrateFlag;
#elif defined(__APPLE__) #elif defined(__APPLE__) || __NUTTX__
cfsetspeed(&portSettings, baudrateFlag); cfsetspeed(&portSettings, baudrateFlag);
#endif #endif
portSettings.c_cflag |= CS8 | CLOCAL | CREAD; portSettings.c_cflag |= CS8 | CLOCAL | CREAD;
@@ -315,7 +315,7 @@ VnError VnSerialPort_close_internal(VnSerialPort *serialport, bool checkAndToggl
serialport->handle = NULL; serialport->handle = NULL;
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__ #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
if (close(serialport->handle) == -1) if (close(serialport->handle) == -1)
return E_UNKNOWN; return E_UNKNOWN;
@@ -338,7 +338,7 @@ VnError VnSerialPort_closeAfterUserUnpluggedSerialPort(VnSerialPort *serialport)
serialport->handle = NULL; serialport->handle = NULL;
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__ #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
if (close(serialport->handle) == -1) if (close(serialport->handle) == -1)
return E_UNKNOWN; return E_UNKNOWN;
@@ -361,7 +361,7 @@ bool VnSerialPort_isOpen(VnSerialPort *serialport)
{ {
#if defined(_WIN32) #if defined(_WIN32)
return serialport->handle != NULL; return serialport->handle != NULL;
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__ #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
return serialport->handle != -1; return serialport->handle != -1;
#else #else
#error "Unknown System" #error "Unknown System"
@@ -395,7 +395,7 @@ VnError VnSerialPort_read(VnSerialPort *serialport, char *buffer, size_t numOfBy
OVERLAPPED overlapped; OVERLAPPED overlapped;
BOOL result; BOOL result;
DWORD numOfBytesTransferred; DWORD numOfBytesTransferred;
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__ #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
int result; int result;
#else #else
#error "Unknown System" #error "Unknown System"
@@ -436,7 +436,7 @@ VnError VnSerialPort_read(VnSerialPort *serialport, char *buffer, size_t numOfBy
if (!result) if (!result)
return E_UNKNOWN; return E_UNKNOWN;
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__ #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
result = read( result = read(
serialport->handle, serialport->handle,
@@ -461,7 +461,7 @@ VnError VnSerialPort_write(VnSerialPort *sp, char const *data, size_t length)
DWORD numOfBytesWritten; DWORD numOfBytesWritten;
BOOL result; BOOL result;
OVERLAPPED overlapped; OVERLAPPED overlapped;
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__ #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
size_t numOfBytesWritten; size_t numOfBytesWritten;
#else #else
#error "Unknown System" #error "Unknown System"
@@ -508,7 +508,7 @@ VnError VnSerialPort_write(VnSerialPort *sp, char const *data, size_t length)
if (!result) if (!result)
return E_UNKNOWN; return E_UNKNOWN;
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__ #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
numOfBytesWritten = write( numOfBytesWritten = write(
sp->handle, sp->handle,
@@ -565,7 +565,7 @@ void VnSerialPort_handleSerialPortNotifications(void* routineData)
sp->handle, sp->handle,
EV_RXCHAR | EV_ERR | EV_RX80FULL); EV_RXCHAR | EV_ERR | EV_RX80FULL);
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__ #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
fd_set readfs; fd_set readfs;
int error; int error;
@@ -672,7 +672,7 @@ void VnSerialPort_handleSerialPortNotifications(void* routineData)
continue; continue;
} }
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__ #elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
FD_ZERO(&readfs); FD_ZERO(&readfs);
FD_SET(sp->handle, &readfs); FD_SET(sp->handle, &readfs);
src/drivers/ins/vectornav/libvnc/src/vn/xplat/thread.c
+14 -1
View File
@@ -7,6 +7,7 @@
#include <unistd.h> #include <unistd.h>
#include <stddef.h> #include <stddef.h>
#include <pthread.h> #include <pthread.h>
#include <sched.h>
#endif #endif
#undef __cplusplus #undef __cplusplus
@@ -77,10 +78,22 @@ VnError VnThread_startNew(VnThread *thread, VnThread_StartRoutine startRoutine,
#elif (defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__) #elif (defined __linux__ || defined __APPLE__ || defined __CYGWIN__ || defined __QNXNTO__ || defined __NUTTX__)
pthread_attr_t attr;
pthread_attr_init(&attr);
pthread_attr_setstacksize(&attr, 5120);
// schedule policy FIFO
pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
// priority
struct sched_param param;
pthread_attr_getschedparam(&attr, &param);
param.sched_priority = sched_get_priority_max(SCHED_FIFO);
pthread_attr_setschedparam(&attr, &param);
errorCode = pthread_create( errorCode = pthread_create(
&(thread->handle), &(thread->handle),
NULL, &attr,
VnThread_intermediateStartRoutine, VnThread_intermediateStartRoutine,
starter); starter);
src/drivers/ins/vectornav/libvnc/src/vn/xplat/time.c
+1 -1
View File
@@ -1,5 +1,5 @@
/* Enable IEEE Std 1003.1b-1993 functionality required for clock_gettime. */ /* Enable IEEE Std 1003.1b-1993 functionality required for clock_gettime. */
#ifdef __linux__ #if defined(__linux__) || defined(__NUTTX__)
/* Works for Ubuntu 15.10 */ /* Works for Ubuntu 15.10 */
#define _POSIX_C_SOURCE 199309L #define _POSIX_C_SOURCE 199309L
#elif defined __CYGWIN__ #elif defined __CYGWIN__