drivers/ins: InertialLabs INS driver bugfix

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 &&
+	// Update sensor_selection for FULL_INS mode
-		hrt_elapsed_time(&time_last_valid_imu_data) < 3_s) {
+	if (_param_ilabs_mode.get() == ILabsMode::FULL_INS && time_initialized != 0 && time_last_valid_imu_data != 0 &&
-		// update sensor_selection if configured in INS mode
+	    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 &&
+	// Missing data handling
-		time_last_valid_imu_data != 0 && hrt_elapsed_time(&time_last_valid_imu_data) > 1_s) {
+	if (time_initialized != 0 && time_last_valid_imu_data != 0 &&
-		PX4_ERR("Timeout, reinitializing");
+	    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

@@ -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

@@ -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};
+	device::Serial  *_serial{nullptr};
-	pthread_t       _threadId;
+	pthread_t        _threadId{0};
-	bool            _processInThread{false};
+	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

@@ -86,72 +86,72 @@ Sensor::~Sensor() {
 }
 
 bool Sensor::init(const char *serialDeviceName, void *context, DataHandler dataHandler) {
-	if (_isInitialized) {
+	if (_isInitialized.load()) {
-		PX4_ERR("Serial device already initialized. Deinitialize first.");
+		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 (!initSerialPort(serialDeviceName)) {
-	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()) {
-		// 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;
+		deinit();
-		px4_sleep(1);  // NOLINT(concurrency-mt-unsafe)
-		resetSerial();
 		return false;
 	}
-
 	pthread_detach(_threadId);
 
-	_isInitialized = true;
+	_isInitialized.store(true);
 	return true;
 }
 
 void Sensor::deinit() {
-	_processInThread = false;
+	if (_isDeinitInProcess.load()) {
-	_isInitialized = false;
+		PX4_WARN("Deinitialization already in process");
-	resetSerial();
+		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();
+		PX4_WARN("Serial port already initialized. Deinitialize first");
-		delete _serial;
+		return false;
-		_serial = nullptr;
 	}
+
+	_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!",
+				PX4_ERR("Unknown message type: %d. Further package parsing result will be incorrect",
-						messageType);
+				        messageType);
 				messageLength = 0;
 				break;
 			}