fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 11:38:56 +08:00
Code Issues Packages Projects Releases Wiki Activity

drivers/ins: InertialLabs INS driver bugfix

Browse Source
This commit is contained in:
Valentin Bugrov
2025-11-11 00:59:59 -05:00
committed by Daniel Agar
parent 3d9905251d
commit 98d3a2141a
4 changed files with 94 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/ins/ilabs/ILabs.cpp
+17 -9
View File
@@ -236,34 +236,42 @@ void ILabs::Run() {
if (!result) {
PX4_ERR("Sensor initializing error");
ScheduleDelayed(1_s);
_sensor.deinit();
_time_initialized.store(0);
_time_last_valid_imu_data.store(0);
ScheduleDelayed(3_s);
return;
}
_time_initialized.store(hrt_absolute_time());
}
// check for timeout
const hrt_abstime time_initialized = _time_initialized.load();
const hrt_abstime time_last_valid_imu_data = _time_last_valid_imu_data.load();
if (_param_ilabs_mode.get() == ILabsMode::FULL_INS && time_last_valid_imu_data != 0 &&
hrt_elapsed_time(&time_last_valid_imu_data) < 3_s) {
// update sensor_selection if configured in INS mode
// Update sensor_selection for FULL_INS mode
if (_param_ilabs_mode.get() == ILabsMode::FULL_INS && time_initialized != 0 && time_last_valid_imu_data != 0 &&
hrt_elapsed_time(&time_last_valid_imu_data) < 3_s) {
if ((_px4_accel.get_device_id() != 0) && (_px4_gyro.get_device_id() != 0)) {
sensor_selection_s sensor_selection{};
sensor_selection.accel_device_id = _px4_accel.get_device_id();
sensor_selection.gyro_device_id = _px4_gyro.get_device_id();
sensor_selection.timestamp = _time_initialized.load();
sensor_selection.timestamp = time_initialized;
_sensor_selection_pub.publish(sensor_selection);
} else {
PX4_ERR("Sensor not initialized");
}
}
if (time_initialized != 0 && hrt_elapsed_time(&time_last_valid_imu_data) > 5_s &&
time_last_valid_imu_data != 0 && hrt_elapsed_time(&time_last_valid_imu_data) > 1_s) {
PX4_ERR("Timeout, reinitializing");
// Missing data handling
if (time_initialized != 0 && time_last_valid_imu_data != 0 &&
hrt_elapsed_time(&time_last_valid_imu_data) > 3_s) {
PX4_ERR("Timeout: no new data from sensor. Reinitializing");
_sensor.deinit();
_time_initialized.store(0);
_time_last_valid_imu_data.store(0);
ScheduleDelayed(3_s);
return;
}
ScheduleDelayed(100_ms);
src/drivers/ins/ilabs/ILabs.h
+4
View File
@@ -80,6 +80,10 @@ private:
void Run() override;
void processData(InertialLabs::SensorsData *sensordata);
static void processDataProxy(void *context, InertialLabs::SensorsData *data) {
if (!context || !data) {
return;
}
ILabs *self = static_cast<ILabs *>(context);
self->processData(data);
}
src/drivers/ins/ilabs/libilabs/include/sensor.h
+10 -5
View File
@@ -35,6 +35,7 @@
#include <pthread.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/Serial.hpp>
#include "data.h"
@@ -61,12 +62,15 @@ public:
private:
static void *updateDataThreadHelper(void *context) {
if (!context) {
return nullptr;
}
Sensor *sensor = reinterpret_cast<Sensor *>(context);
sensor->updateData();
return nullptr;
}
void resetSerial();
bool initSerialPort(const char *serialDeviceName);
bool moveToBufferStart(const uint8_t *pos);
bool skipPackageInBufferStart();
bool movePackageHeaderToBufferStart();
@@ -74,15 +78,16 @@ private:
bool parseUDDPayload();
device::Serial *_serial{nullptr};
pthread_t _threadId;
bool _processInThread{false};
device::Serial *_serial{nullptr};
pthread_t _threadId{0};
px4::atomic_bool _processInThread{false};
// callback. C-style class method pointer
void *_context{nullptr};
DataHandler _dataHandler{nullptr};
bool _isInitialized{false};
px4::atomic_bool _isInitialized{false};
px4::atomic_bool _isDeinitInProcess{false};
uint8_t _buf[BUFFER_SIZE]{};
uint16_t _bufOffset{0};
src/drivers/ins/ilabs/libilabs/src/sensor.cpp
+63 -41
View File
@@ -86,72 +86,72 @@ Sensor::~Sensor() {
}
bool Sensor::init(const char *serialDeviceName, void *context, DataHandler dataHandler) {
if (_isInitialized) {
PX4_ERR("Serial device already initialized. Deinitialize first.");
if (_isInitialized.load()) {
PX4_WARN("Serial device already initialized. Deinitialize first");
return false;
}
if (_isDeinitInProcess.load()) {
PX4_WARN("Deinitialization is in progress. Please wait until it completes before re-initializing");
return false;
}
if (serialDeviceName[0] == '\0') {
PX4_ERR("Empty serial device name");
return false;
}
if (!context || !dataHandler) {
PX4_ERR("Empty data handler callback");
return false;
}
_serial = new device::Serial(serialDeviceName, BAUDRATE);
if (_serial == nullptr) {
PX4_ERR("Error creating serial device: %s", serialDeviceName);
px4_sleep(1); // NOLINT(concurrency-mt-unsafe)
if (!initSerialPort(serialDeviceName)) {
return false;
}
if (!_serial->isOpen() && !_serial->open()) {
// Open the UART. If this is successful then the UART is ready to use.
if (!_serial->open()) {
PX4_ERR("Error opening serial device: %s", serialDeviceName);
px4_sleep(1); // NOLINT(concurrency-mt-unsafe)
resetSerial();
return false;
}
PX4_INFO("Serial device opened sucessfully: %s", serialDeviceName);
_serial->flush();
}
_context = context;
_dataHandler = dataHandler;
_processInThread = true;
_processInThread.store(true);
const int result = pthread_create(&_threadId, nullptr, &Sensor::updateDataThreadHelper, this);
if (result) {
PX4_ERR("Cant create working thread");
_processInThread = false;
px4_sleep(1); // NOLINT(concurrency-mt-unsafe)
resetSerial();
deinit();
return false;
}
pthread_detach(_threadId);
_isInitialized = true;
_isInitialized.store(true);
return true;
}
void Sensor::deinit() {
_processInThread = false;
_isInitialized = false;
resetSerial();
if (_isDeinitInProcess.load()) {
PX4_WARN("Deinitialization already in process");
return;
}
_isDeinitInProcess.store(true);
_isInitialized.store(false);
_processInThread.store(false);
// Wait to be shure, that operations with UART are finished
px4_sleep(1); // NOLINT(concurrency-mt-unsafe)
if (_serial) {
(void)_serial->close();
delete _serial;
_serial = nullptr;
}
_context = nullptr;
_dataHandler = nullptr;
_bufOffset = 0;
_isDeinitInProcess.store(false);
}
bool Sensor::isInitialized() const {
return _isInitialized;
return _isInitialized.load();
}
void Sensor::updateData() {
while (_processInThread) {
while (_processInThread.load()) {
bool result = moveValidPackageToBufferStart();
if (!result) {
continue;
@@ -162,18 +162,40 @@ void Sensor::updateData() {
continue;
}
_dataHandler(_context, &_sensorData);
if (_dataHandler) {
_dataHandler(_context, &_sensorData);
}
skipPackageInBufferStart();
}
}
void Sensor::resetSerial() {
bool Sensor::initSerialPort(const char *serialDeviceName) {
if (_serial) {
(void)_serial->close();
delete _serial;
_serial = nullptr;
PX4_WARN("Serial port already initialized. Deinitialize first");
return false;
}
_serial = new device::Serial(serialDeviceName, BAUDRATE);
if (_serial == nullptr) {
PX4_ERR("Error creating serial device: %s", serialDeviceName);
px4_sleep(1); // NOLINT(concurrency-mt-unsafe)
return false;
}
if (!_serial->isOpen()) {
_serial->open();
if (!_serial->isOpen()) {
PX4_ERR("Error opening serial device: %s", serialDeviceName);
deinit();
return false;
}
PX4_INFO("Serial device opened sucessfully: %s", serialDeviceName);
} else {
PX4_INFO("Serial device already opened: %s", serialDeviceName);
}
_serial->flush();
return true;
}
bool Sensor::moveToBufferStart(const uint8_t *pos) {
@@ -201,7 +223,7 @@ bool Sensor::moveToBufferStart(const uint8_t *pos) {
bool Sensor::skipPackageInBufferStart() {
const MessageHeader *packageHeader = reinterpret_cast<MessageHeader *>(_buf);
if (!isMessageHeaderCorrect(packageHeader)) {
PX4_ERR("Incorrect message header in buffer start. Can't skip the package correctly.");
PX4_ERR("Incorrect message header in buffer start. Can't skip the package correctly");
return false;
}
@@ -239,7 +261,7 @@ bool Sensor::moveValidPackageToBufferStart() {
return false;
}
const ssize_t nRead = _serial->readAtLeast(&_buf[_bufOffset], freeBufByteCount, freeBufByteCount, 1000);
const ssize_t nRead = _serial->readAtLeast(&_buf[_bufOffset], freeBufByteCount, freeBufByteCount, 500);
if (nRead != freeBufByteCount) {
PX4_ERR("Can't read requested data from the serial device. Bytes requested: %d. Bytes read: %d",
freeBufByteCount, static_cast<int>(nRead));
@@ -546,8 +568,8 @@ bool Sensor::parseUDDPayload() {
break;
}
default: {
PX4_ERR("Unknown message type: %d. Further package parsing result will be incorrect!",
messageType);
PX4_ERR("Unknown message type: %d. Further package parsing result will be incorrect",
messageType);
messageLength = 0;
break;
}