dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

- 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
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
11 changed files with 81 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
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 ttyS0{"wq:ttyS0", 1632, -21};
static constexpr wq_config_t ttyS1{"wq:ttyS1", 1632, -22};
static constexpr wq_config_t ttyS2{"wq:ttyS2", 1632, -23};
static constexpr wq_config_t ttyS3{"wq:ttyS3", 1632, -24};
static constexpr wq_config_t ttyS4{"wq:ttyS4", 1632, -25};
static constexpr wq_config_t ttyS5{"wq:ttyS5", 1632, -26};
static constexpr wq_config_t ttyS6{"wq:ttyS6", 1632, -27};
static constexpr wq_config_t ttyS7{"wq:ttyS7", 1632, -28};
static constexpr wq_config_t ttyS8{"wq:ttyS8", 1632, -29};
static constexpr wq_config_t ttyS9{"wq:ttyS9", 1632, -30};
static constexpr wq_config_t ttyACM0{"wq:ttyACM0", 1632, -31};
static constexpr wq_config_t ttyUnknown{"wq:ttyUnknown", 1632, -32};
static constexpr wq_config_t ttyS0{"wq:ttyS0", 1728, -21};
static constexpr wq_config_t ttyS1{"wq:ttyS1", 1728, -22};
static constexpr wq_config_t ttyS2{"wq:ttyS2", 1728, -23};
static constexpr wq_config_t ttyS3{"wq:ttyS3", 1728, -24};
static constexpr wq_config_t ttyS4{"wq:ttyS4", 1728, -25};
static constexpr wq_config_t ttyS5{"wq:ttyS5", 1728, -26};
static constexpr wq_config_t ttyS6{"wq:ttyS6", 1728, -27};
static constexpr wq_config_t ttyS7{"wq:ttyS7", 1728, -28};
static constexpr wq_config_t ttyS8{"wq:ttyS8", 1728, -29};
static constexpr wq_config_t ttyS9{"wq:ttyS9", 1728, -30};
static constexpr wq_config_t ttyACM0{"wq:ttyACM0", 1728, -31};
static constexpr wq_config_t ttyUnknown{"wq:ttyUnknown", 1728, -32};
static constexpr wq_config_t lp_default{"wq:lp_default", 1920, -50};

35
src/drivers/ins/vectornav/VectorNav.cpp
View File

@ -115,7 +115,8 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
GPSGROUP_NONE)
) {
// TIMEGROUP_TIMESTARTUP
//uint64_t time_startup = VnUartPacket_extractUint64(packet);
uint64_t time_startup = VnUartPacket_extractUint64(packet);
(void)time_startup;
// IMUGROUP_UNCOMPACCEL
vec3f accel = VnUartPacket_extractVec3f(packet);
@ -141,7 +142,8 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
GPSGROUP_NONE)
) {
// TIMEGROUP_TIMESTARTUP
//const uint64_t time_startup = VnUartPacket_extractUint64(packet);
const uint64_t time_startup = VnUartPacket_extractUint64(packet);
(void)time_startup;
// IMUGROUP_TEMP
const float temperature = VnUartPacket_extractFloat(packet);
@ -276,7 +278,8 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
GPSGROUP_NONE)
) {
// TIMEGROUP_TIMESTARTUP
//const uint64_t time_startup = VnUartPacket_extractUint64(packet);
const uint64_t time_startup = VnUartPacket_extractUint64(packet);
(void)time_startup;
// GPSGROUP_UTC
// TimeUtc timeUtc;
@ -358,6 +361,8 @@ bool VectorNav::init()
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};
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;
// change baudrate to max
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
@ -385,6 +398,7 @@ bool VectorNav::init()
if ((error = VnSensor_readModelNumber(&_vs, model_number, sizeof(model_number))) != E_NONE) {
PX4_ERR("Error reading model number %d", error);
VnSensor_disconnect(&_vs);
return false;
}
@ -393,6 +407,7 @@ bool VectorNav::init()
if ((error = VnSensor_readHardwareRevision(&_vs, &hardware_revision)) != E_NONE) {
PX4_ERR("Error reading HW revision %d", error);
VnSensor_disconnect(&_vs);
return false;
}
@ -401,6 +416,7 @@ bool VectorNav::init()
if ((error = VnSensor_readSerialNumber(&_vs, &serial_number)) != E_NONE) {
PX4_ERR("Error reading serial number %d", error);
VnSensor_disconnect(&_vs);
return false;
}
@ -409,6 +425,7 @@ bool VectorNav::init()
if ((error = VnSensor_readFirmwareVersion(&_vs, firmware_version, sizeof(firmware_version))) != E_NONE) {
PX4_ERR("Error reading firmware version %d", error);
VnSensor_disconnect(&_vs);
return false;
}
@ -456,7 +473,6 @@ bool VectorNav::configure()
PX4_ERR("Error reading VPE basic control %d", error);
}
VnError VnSensor_readImuFilteringConfiguration(VnSensor * sensor, ImuFilteringConfigurationRegister * fields);
// VnError VnSensor_writeImuFilteringConfiguration(VnSensor *sensor, ImuFilteringConfigurationRegister fields, bool waitForReply);
@ -479,7 +495,7 @@ bool VectorNav::configure()
// binary output 1: max rate IMU
BinaryOutputRegister_initialize(
&_binary_output_group_1,
ASYNCMODE_PORT1,
ASYNCMODE_PORT2,
1, // divider
COMMONGROUP_NONE,
TIMEGROUP_TIMESTARTUP,
@ -502,7 +518,7 @@ bool VectorNav::configure()
// binary output 2: medium rate AHRS, INS, baro, mag
BinaryOutputRegister_initialize(
&_binary_output_group_2,
ASYNCMODE_PORT1,
ASYNCMODE_PORT2,
8, // divider
COMMONGROUP_NONE,
TIMEGROUP_TIMESTARTUP,
@ -521,7 +537,7 @@ bool VectorNav::configure()
// binary output 3: low rate GNSS
BinaryOutputRegister_initialize(
&_binary_output_group_3,
ASYNCMODE_PORT1,
ASYNCMODE_PORT2,
80, // divider
COMMONGROUP_NONE,
TIMEGROUP_TIMESTARTUP,
@ -577,9 +593,6 @@ void VectorNav::Run()
}
}
ScheduleDelayed(100_ms);
}

2
src/drivers/ins/vectornav/libvnc/include/vn/enum.h
View File

@ -667,4 +667,4 @@ typedef enum
VNPROCESSOR_IMU /**< IMU Processor. */
} VnProcessorType;
#endif
#endif

4
src/drivers/ins/vectornav/libvnc/include/vn/xplat/serialport.h
View File

@ -26,9 +26,9 @@
#endif
#endif
#ifdef __linux__
#if defined(__linux__)
#include <linux/serial.h>
#elif defined __APPLE__
#elif defined __APPLE__ || defined(__NUTTX__)
#include <dirent.h>
#endif

2
src/drivers/ins/vectornav/libvnc/include/vn/xplat/time.h
View File

@ -39,7 +39,7 @@ typedef struct
#if _WIN32
double pcFrequency;
__int64 counterStart;
#elif __linux__ || __APPLE__ ||__CYGWIN__ || __QNXNTO__ || defined __NUTTX__
#elif __linux__ || __APPLE__ ||__CYGWIN__ || __QNXNTO__ || __NUTTX__
double clockStart;
#else
#error "Unknown System"

8
src/drivers/ins/vectornav/libvnc/src/vn/sensors/searcher.c
View File

@ -9,7 +9,7 @@
#include <stdlib.h>
#ifdef __linux__
#if defined(__linux__) || defined(__NUTTX__)
#include <errno.h>
#include <fcntl.h>
#include <unistd.h>
@ -310,7 +310,7 @@ int32_t VnSearcher_getPortBaud(VnPortInfo* portInfo)
return portInfo->baud;
}
#ifdef __linux__
#if defined(__linux__) || defined(__NUTTX__)
void VnSearcher_findPorts_LINUX(char*** portNamesOut, int32_t* numPortsFound)
{
char portName[15] = {0};
@ -330,9 +330,9 @@ void VnSearcher_findPorts_LINUX(char*** portNamesOut, int32_t* numPortsFound)
/* Attempt to open the serial port */
portFd = open(portName,
#if __linux__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
#if __linux__ || __CYGWIN__ || __QNXNTO__
O_RDWR | O_NOCTTY );
#elif __APPLE__
#elif __APPLE__ || __NUTTX__
O_RDWR | O_NOCTTY | O_NONBLOCK);
#else
#error "Unknown System"

8
src/drivers/ins/vectornav/libvnc/src/vn/xplat/criticalsection.c
View File

@ -6,7 +6,7 @@ VnError VnCriticalSection_initialize(VnCriticalSection *cs)
InitializeCriticalSection(&cs->handle);
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
if (pthread_mutex_init(&cs->handle, NULL))
return E_UNKNOWN;
@ -24,7 +24,7 @@ VnError VnCriticalSection_deinitialize(VnCriticalSection *cs)
DeleteCriticalSection(&cs->handle);
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
if (pthread_mutex_destroy(&cs->handle))
return E_UNKNOWN;
@ -42,7 +42,7 @@ VnError VnCriticalSection_enter(VnCriticalSection *cs)
EnterCriticalSection(&cs->handle);
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
if (pthread_mutex_lock(&cs->handle))
return E_UNKNOWN;
@ -60,7 +60,7 @@ VnError VnCriticalSection_leave(VnCriticalSection *cs)
LeaveCriticalSection(&cs->handle);
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
if (pthread_mutex_unlock(&cs->handle))
return E_UNKNOWN;

6
src/drivers/ins/vectornav/libvnc/src/vn/xplat/event.c
View File

@ -1,5 +1,5 @@
/* Enable IEEE Std 1003.1b-1993 functionality required for clock_gettime. */
#ifdef __linux__
#if defined(__linux__) || defined(__NUTTX__)
/* Works for Ubuntu 15.10 */
#define _POSIX_C_SOURCE 199309L
#elif defined __CYGWIN__
@ -100,7 +100,7 @@ VnError VnEvent_waitMs(VnEvent *e, uint32_t timeoutMs)
return E_UNKNOWN;
#elif defined __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
#elif defined __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
return VnEvent_waitUs(e, timeoutMs * 1000);
@ -218,7 +218,7 @@ VnError VnEvent_signal(VnEvent *e)
if (!SetEvent(e->handle))
return E_UNKNOWN;
#elif defined __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
#elif defined __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
if (pthread_mutex_lock(&e->mutex))
return E_UNKNOWN;

30
src/drivers/ins/vectornav/libvnc/src/vn/xplat/serialport.c
View File

@ -2,7 +2,7 @@
#if _WIN32
/* Nothing to do. */
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
@ -31,7 +31,7 @@ VnError VnSerialPort_initialize(VnSerialPort *sp)
#if _WIN32
sp->handle = NULL;
VnCriticalSection_initialize(&sp->readWriteCS);
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
sp->handle = -1;
#else
#error "Unknown System"
@ -196,9 +196,9 @@ VnError VnSerialPort_open_internal(VnSerialPort *serialport, char const *portNam
portFd = open(
portName,
#if __linux__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
#if __linux__ || __CYGWIN__ || __QNXNTO__
O_RDWR | O_NOCTTY);
#elif __APPLE__
#elif __APPLE__ || __NUTTX__
O_RDWR | O_NOCTTY | O_NONBLOCK);
#else
#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. */
#if __linux__ || __QNXNTO__ || __CYGWIN__ || defined __NUTTX__
#if __linux__ || __QNXNTO__ || __CYGWIN__
portSettings.c_cflag = baudrateFlag;
#elif defined(__APPLE__)
#elif defined(__APPLE__) || __NUTTX__
cfsetspeed(&portSettings, baudrateFlag);
#endif
portSettings.c_cflag |= CS8 | CLOCAL | CREAD;
@ -315,7 +315,7 @@ VnError VnSerialPort_close_internal(VnSerialPort *serialport, bool checkAndToggl
serialport->handle = NULL;
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
if (close(serialport->handle) == -1)
return E_UNKNOWN;
@ -338,7 +338,7 @@ VnError VnSerialPort_closeAfterUserUnpluggedSerialPort(VnSerialPort *serialport)
serialport->handle = NULL;
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
if (close(serialport->handle) == -1)
return E_UNKNOWN;
@ -361,7 +361,7 @@ bool VnSerialPort_isOpen(VnSerialPort *serialport)
{
#if defined(_WIN32)
return serialport->handle != NULL;
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
return serialport->handle != -1;
#else
#error "Unknown System"
@ -395,7 +395,7 @@ VnError VnSerialPort_read(VnSerialPort *serialport, char *buffer, size_t numOfBy
OVERLAPPED overlapped;
BOOL result;
DWORD numOfBytesTransferred;
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
int result;
#else
#error "Unknown System"
@ -436,7 +436,7 @@ VnError VnSerialPort_read(VnSerialPort *serialport, char *buffer, size_t numOfBy
if (!result)
return E_UNKNOWN;
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
result = read(
serialport->handle,
@ -461,7 +461,7 @@ VnError VnSerialPort_write(VnSerialPort *sp, char const *data, size_t length)
DWORD numOfBytesWritten;
BOOL result;
OVERLAPPED overlapped;
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
size_t numOfBytesWritten;
#else
#error "Unknown System"
@ -508,7 +508,7 @@ VnError VnSerialPort_write(VnSerialPort *sp, char const *data, size_t length)
if (!result)
return E_UNKNOWN;
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
numOfBytesWritten = write(
sp->handle,
@ -565,7 +565,7 @@ void VnSerialPort_handleSerialPortNotifications(void* routineData)
sp->handle,
EV_RXCHAR | EV_ERR | EV_RX80FULL);
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
fd_set readfs;
int error;
@ -672,7 +672,7 @@ void VnSerialPort_handleSerialPortNotifications(void* routineData)
continue;
}
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || defined __NUTTX__
#elif __linux__ || __APPLE__ || __CYGWIN__ || __QNXNTO__ || __NUTTX__
FD_ZERO(&readfs);
FD_SET(sp->handle, &readfs);

15
src/drivers/ins/vectornav/libvnc/src/vn/xplat/thread.c
View File

@ -7,6 +7,7 @@
#include <unistd.h>
#include <stddef.h>
#include <pthread.h>
#include <sched.h>
#endif
#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__)
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(
&(thread->handle),
NULL,
&attr,
VnThread_intermediateStartRoutine,
starter);

2
src/drivers/ins/vectornav/libvnc/src/vn/xplat/time.c
View File

@ -1,5 +1,5 @@
/* Enable IEEE Std 1003.1b-1993 functionality required for clock_gettime. */
#ifdef __linux__
#if defined(__linux__) || defined(__NUTTX__)
/* Works for Ubuntu 15.10 */
#define _POSIX_C_SOURCE 199309L
#elif defined __CYGWIN__