diff --git a/src/drivers/ins/ilabs/ILabs.cpp b/src/drivers/ins/ilabs/ILabs.cpp index 98f169357d..1c81a818ab 100644 --- a/src/drivers/ins/ilabs/ILabs.cpp +++ b/src/drivers/ins/ilabs/ILabs.cpp @@ -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); diff --git a/src/drivers/ins/ilabs/ILabs.h b/src/drivers/ins/ilabs/ILabs.h index 82b94541db..3754dce982 100644 --- a/src/drivers/ins/ilabs/ILabs.h +++ b/src/drivers/ins/ilabs/ILabs.h @@ -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(context); self->processData(data); } diff --git a/src/drivers/ins/ilabs/libilabs/include/sensor.h b/src/drivers/ins/ilabs/libilabs/include/sensor.h index f8c29bef0a..504cabbc53 100644 --- a/src/drivers/ins/ilabs/libilabs/include/sensor.h +++ b/src/drivers/ins/ilabs/libilabs/include/sensor.h @@ -35,6 +35,7 @@ #include +#include #include #include "data.h" @@ -61,12 +62,15 @@ public: private: static void *updateDataThreadHelper(void *context) { + if (!context) { + return nullptr; + } Sensor *sensor = reinterpret_cast(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}; diff --git a/src/drivers/ins/ilabs/libilabs/src/sensor.cpp b/src/drivers/ins/ilabs/libilabs/src/sensor.cpp index fa79551188..ba6427536c 100644 --- a/src/drivers/ins/ilabs/libilabs/src/sensor.cpp +++ b/src/drivers/ins/ilabs/libilabs/src/sensor.cpp @@ -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(_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(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; }