Murata UART driver + fixes to pps fake.

.github/workflows/clang-format.yml

@@ -12,10 +12,15 @@ jobs:
       - uses: actions/checkout@v4
 
       - name: Install clang-format
-        run: sudo apt-get install -y clang-format
+        run: |
+          sudo apt-get update
+          sudo apt-get install -y clang-format
+          clang-format --version
 
       - name: Check formatting
         run: |
-          find code/ extras/ -name "*.cpp" -o -name "*.h" \
+          find code/ extras/ \
+            -path "extras/oled_status/u8g8-arm" -prune -o \
+            \( -name "*.cpp" -o -name "*.h" \) -print \
             | grep -v "cmake-build" \
-            | xargs clang-format --dry-run --Werror
+            | xargs clang-format-18 --dry-run --Werror

CMakeLists.txt

@@ -164,17 +164,13 @@ add_executable(led_demo code/led_demo.cpp code/gpios.cpp)
 set(CMAKE_CXX_LINK_FLAGS "${CMAKE_CXX_LINK_FLAGS}")
 target_link_libraries(led_demo gpiod pthread)
 
-add_executable(fake_pps code/fake_pps.cpp code/gpios.cpp)
-set(CMAKE_CXX_LINK_FLAGS "${CMAKE_CXX_LINK_FLAGS}")
-target_link_libraries(fake_pps ${LIBSERIAL_LIBRARY} gpiod pthread)
-
 
 add_executable(button_demo code/button_demo.cpp code/gpios.cpp)
 set(CMAKE_CXX_LINK_FLAGS "${CMAKE_CXX_LINK_FLAGS}")
 target_link_libraries(button_demo gpiod pthread)
 
 # Define install directories
-install(TARGETS control_program led_demo fake_pps button_demo
+install(TARGETS control_program led_demo button_demo
         RUNTIME DESTINATION /opt/mandeye/)
 
 set(MANDEYE_USE_LIBCAMERA ON CACHE BOOL "Build libcamera extra")
@@ -184,13 +180,18 @@ if(MANDEYE_USE_LIBCAMERA)
     add_subdirectory(extras/libcamera)
 endif()
 add_subdirectory(extras/rtkNmea)
+set(MANDEYE_BUILD_EXTRAS_MURATA_UART_IMU ON CACHE BOOL "Build Murata UART IMU extra module - Experimental")
+if (MANDEYE_BUILD_EXTRAS_MURATA_UART_IMU)
+    message(STATUS "Building Murata SPI IMU extra module")
+    add_subdirectory(extras/MurataUartImu)
+endif()
+
 add_subdirectory(extras/FakePPS)
 add_subdirectory(extras/SlavePPS)
 add_subdirectory(extras/oled_status)
 
 install(FILES packing/helpers.sh DESTINATION /opt/mandeye/)
 install(FILES packing/services/mandeye_controller.service  DESTINATION /usr/lib/systemd/system)
-install(FILES packing/services/mandeye_fake_pps.service  DESTINATION /usr/lib/systemd/system)
 
 install(FILES packing/services/mandeye_phc2sys-gm-eth0.service DESTINATION /usr/lib/systemd/system)
 install(FILES packing/services/mandeye_ptp4l-gm-eth0.service DESTINATION /usr/lib/systemd/system)
@@ -203,4 +204,4 @@ set(CPACK_PACKAGE_VERSION "${MANDEYE_VERSION}-${GIT_HASH}-${MANDEYE_HARDWARE_HEA
 set(CPACK_PACKAGE_CONTACT "Michał Pełka <michalpelka@gmail.com>")
 set(CPACK_DEBIAN_PACKAGE_DEPENDS "rapidjson-dev,libserial-dev, libgpiod-dev, libzmq3-dev")
 set(CPACK_DEBIAN_PACKAGE_CONTROL_EXTRA "${CMAKE_CURRENT_SOURCE_DIR}/packing/postinst/postinst")
-include(CPack)
+include(CPack)

README.md

@@ -300,6 +300,11 @@ sudo systemctl enable mandeye_extra_gnss.service
 mandeye_extra_gnss_start
 ```
 
+Extra muRata UART IMU (experimental):
+```shell
+sudo systemctl enable mandeye_murata_uart_driver.service
+mandeye_murata_uart_imu_start
+```
 # Services
 List state of services:
 ```shell

code/fake_pps.cpp

@@ -1,176 +0,0 @@
-#include <atomic>
-#include <chrono>
-#include <gpios.h>
-#include <iostream>
-#include <stdio.h>
-#include <thread>
-
-#include <SerialPort.h>
-#include <SerialStream.h>
-#include <gpiod.h>
-#include <hardware_config/mandeye.h>
-
-#include "hardware_config/mandeye.h"
-
-namespace NMEA
-{
-const unsigned int BufferLen = 128;
-
-//! Helper structure to get time
-struct timestamp
-{
-	uint8_t hours;
-	uint8_t mins;
-	uint8_t secs;
-	uint8_t day;
-	uint8_t month;
-	uint8_t year;
-};
-
-std::string produceNMEA(const NMEA::timestamp& ts)
-{
-	char buffer[BufferLen];
-	char payload[BufferLen];
-	snprintf(
-		//		payload, NMEA::BufferLen, "GPRMC,%02d%02d%02d.00,A,5109.0262308,N,11401.8407342,W,0.004,133.4,%s,0.0,E,D", ts.hours, ts.mins, ts.secs, date);
-		payload,
-		NMEA::BufferLen,
-		"GPRMC,%02d%02d%02d.00,A,5109.0262308,N,11401.8407342,W,0.004,133.4,%02d%02d%02d,0.0,E,D",
-		ts.hours,
-		ts.mins,
-		ts.secs,
-		ts.day,
-		ts.month,
-		ts.year);
-	size_t len = strnlen(payload, NMEA::BufferLen);
-	// compute NMEA checksum on buffer
-	uint8_t NMEAChecksumComputed = 0;
-
-	size_t i = 0;
-	for(i = 0; i < len; i++)
-	{
-		NMEAChecksumComputed ^= payload[i];
-	}
-	// attach cheksum
-	snprintf(buffer, NMEA::BufferLen, "$%s*%02X\n", payload, NMEAChecksumComputed);
-	return std::string(buffer);
-}
-
-NMEA::timestamp GetTimestampFromSec(time_t secsElapsed)
-{
-	std::tm* timeInfo = gmtime(&secsElapsed);
-	NMEA::timestamp ts;
-	ts.hours = timeInfo->tm_hour;
-	ts.mins = timeInfo->tm_min;
-	ts.secs = timeInfo->tm_sec;
-	ts.day = timeInfo->tm_mday;
-	ts.month = timeInfo->tm_mon + 1;
-	ts.year = timeInfo->tm_year - 100;
-	return ts;
-}
-} // namespace NMEA
-
-std::atomic<bool> stop{false};
-void oneSecondThread()
-{
-	// setup serial port
-	std::vector<std::unique_ptr<LibSerial::SerialPort>> serialPorts;
-	std::vector<gpiod_line*> syncOutsLines;
-
-	const auto portsNames = hardware::GetLidarSyncPorts();
-	for(const auto& portName : portsNames)
-	{
-		std::cout << "opening port " << portName << std::endl;
-		std::unique_ptr<LibSerial::SerialPort> serialPort = std::make_unique<LibSerial::SerialPort>();
-		serialPort->Open(portName, std::ios_base::out);
-		serialPort->SetBaudRate(LibSerial::BaudRate::BAUD_9600);
-		serialPorts.emplace_back(std::move(serialPort));
-	}
-	const auto ouputs = hardware::GetLidarSyncGPIO();
-	const auto& chipPath = hardware::GetGPIOChip();
-	std::cout << "Opening GPIO chip " << chipPath << std::endl;
-
-	gpiod_chip* chip = gpiod_chip_open(chipPath);
-	if(chip == nullptr)
-	{
-		std::cerr << "Error: Unable to open GPIO chip." << std::endl;
-		std::abort();
-	}
-
-	for(const auto& pin : ouputs)
-	{
-		auto line = gpiod_chip_get_line(chip, pin);
-		if(line == nullptr)
-		{
-			std::cerr << "Error: Unable to open GPIO line." << std::endl;
-			gpiod_chip_close(chip);
-			std::abort();
-		}
-		int ret = gpiod_line_request_output(line, "mandeye_fake_pps", 0);
-		if(ret < 0)
-		{
-			std::cerr << "Error: Unable to request GPIO line." << std::endl;
-			gpiod_chip_close(chip);
-			std::abort();
-		}
-		syncOutsLines.emplace_back(line);
-	}
-	assert(serialPorts.size() == syncOutsLines.size());
-
-	//setup pps gpio
-	constexpr uint64_t Rate = 1000;
-	const auto now = std::chrono::system_clock::now();
-	uint64_t millisFromEpoch = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count();
-	millisFromEpoch += Rate;
-
-	//round to next second
-	millisFromEpoch = (millisFromEpoch / Rate) * Rate;
-	auto waKeUpTime = std::chrono::system_clock::time_point(std::chrono::milliseconds(millisFromEpoch));
-
-	while(!stop)
-	{
-		std::this_thread::sleep_until(waKeUpTime);
-		auto currentTime = std::chrono::system_clock::now();
-		millisFromEpoch += Rate;
-
-		waKeUpTime = std::chrono::system_clock::time_point(std::chrono::milliseconds(millisFromEpoch));
-
-		const uint64_t secs = millisFromEpoch / 1000;
-		NMEA::timestamp ts = NMEA::GetTimestampFromSec(secs);
-
-		for(auto& syncOut : syncOutsLines)
-		{
-			gpiod_line_set_value(syncOut, 0);
-		}
-		std::this_thread::sleep_for(std::chrono::milliseconds(10));
-		for(auto& syncOut : syncOutsLines)
-		{
-			gpiod_line_set_value(syncOut, 1);
-		}
-		std::this_thread::sleep_for(std::chrono::milliseconds(100));
-		const std::string nmeaMessage = NMEA::produceNMEA(ts);
-		for(auto& serialPort : serialPorts)
-		{
-			serialPort->Write(nmeaMessage);
-		}
-
-		std::this_thread::sleep_until(waKeUpTime);
-	}
-	for(auto& syncOut : syncOutsLines)
-	{
-		gpiod_line_release(syncOut);
-	}
-	gpiod_chip_close(chip);
-}
-int main(int arc, char* argv[])
-{
-	std::cout << "fake pps" << std::endl;
-
-	std::thread t1(oneSecondThread);
-	while(!stop)
-	{
-		std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-	}
-	t1.join();
-	return 0;
-}

code/hardware_config/mandeye-standard-rpi5.h

@@ -116,7 +116,7 @@ constexpr GPIO::GPIO_PULL GetPULL([[maybe_unused]] BUTTON btn)
 
 [[maybe_unused]] inline const std::string GetGNSSPort()
 {
-	return "/dev/ttyAMA0";
+	return "";
 };
 
 [[maybe_unused]] inline const LibSerial::BaudRate GetGNSSBaudrate()

code/lidars/hesai/HesaiClient.cpp

@@ -170,8 +170,7 @@ void HesaiClient::CallbackFrame(const LidarDecodedFrame<LidarPointXYZICRT>& data
 			data.z = point.z;
 			data.intensity = point.intensity;
 			data.laser_id = 0;
-			data.timestamp = point.timestamp;
-			m_timestamp = point.timestamp;
+			data.timestamp = point.timestamp * 1e9; // convert to nanosecs
 			m_bufferLidarPtr->push_back(data);
 		}
 	}
@@ -188,13 +187,13 @@ void HesaiClient::CallbackIMU(const LidarImuData& dataFrame)
 		auto millis = std::chrono::duration_cast<std::chrono::milliseconds>(duration);
 
 		LidarIMU data;
-		data.timestamp = dataFrame.timestamp;
+		data.timestamp = dataFrame.timestamp * 1e9;
 		data.acc_x = dataFrame.imu_accel_x;
 		data.acc_y = dataFrame.imu_accel_y;
 		data.acc_z = dataFrame.imu_accel_z;
-		data.gyro_x = dataFrame.imu_ang_vel_x;
-		data.gyro_y = dataFrame.imu_ang_vel_y;
-		data.gyro_z = dataFrame.imu_ang_vel_z;
+		data.gyro_x = M_PI * dataFrame.imu_ang_vel_x / 180.0; // to rads
+		data.gyro_y = M_PI * dataFrame.imu_ang_vel_y / 180.0;
+		data.gyro_z = M_PI * dataFrame.imu_ang_vel_z / 180.0;
 		data.laser_id = 0;
 		data.epoch_time = millis.count();
 		m_bufferIMUPtr->push_back(data);

code/lidars/livoxsdk2/LivoxClient.cpp

@@ -97,7 +97,7 @@ nlohmann::json LivoxClient::produceStatus()
 	data["LivoxLidarInfo"]["m_sessionStart_s"] = double(m_sessionStart.value_or(-1.f)) / 1e9;
 	data["LivoxLidarInfo"]["m_elapsed"] = m_elapsed;
 	data["LivoxLidarInfo"]["m_elapsed_s"] = double(m_elapsed) / 1e9;
-
+	data["LivoxLidarInfo"]["m_time_diff"] = m_time_diff;
 	auto arrayworkMode = nlohmann::json::array();
 	for(auto& mode : m_LivoxLidarWorkMode)
 	{
@@ -184,8 +184,9 @@ std::pair<LidarPointsBufferPtr, LidarIMUBufferPtr> LivoxClient::retrieveData()
 }
 void LivoxClient::testThread()
 {
-	std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+	std::this_thread::sleep_for(std::chrono::milliseconds(1000));
 	std::cout << "Livox periodical watch thread" << std::endl;
+
 	while(!isDone)
 	{
 		std::this_thread::sleep_for(std::chrono::milliseconds(1000));
@@ -247,6 +248,11 @@ void LivoxClient::saveTimeStamp(LivoxClient* client, uint64_t timestamp)
 	{
 		client->m_elapsed = client->m_timestamp - client->m_sessionStart.value();
 	}
+	using namespace std::chrono;
+	const auto now = system_clock::now();
+	auto duration = now.time_since_epoch();
+	double tp = std::chrono::duration<double>(duration).count();
+	client->m_time_diff = std::abs(tp - double(client->m_timestamp) / 1e9);
 }
 
 void LivoxClient::PointCloudCallback(uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket* data, void* client_data)
@@ -324,7 +330,6 @@ bool LivoxClient::isSynced()
 			return false;
 		}
 	}
-	std::cout << "Time sync is synced" << std::endl;
 	return true;
 }
 
@@ -339,6 +344,8 @@ void LivoxClient::ImuDataCallback(uint32_t handle, const uint8_t dev_type, Livox
 	auto millis = std::chrono::duration_cast<std::chrono::milliseconds>(duration);
 
 	LivoxClient* this_ptr = (LivoxClient*)client_data;
+	// std::cout << "m_time_diff " <<this_ptr->m_time_diff << std::endl;
+
 	if(data->data_type == kLivoxLidarImuData)
 	{
 		const auto laser_id = this_ptr->handleToLidarId(handle);

code/lidars/livoxsdk2/LivoxClient.h

@@ -67,7 +67,7 @@ class LivoxClient : public BaseLidarClient
 
 	std::unordered_map<uint32_t, uint64_t> m_handleToLastTimestamp;
 	std::unordered_map<uint32_t, std::string> m_handleToSerialNumber;
-
+	double m_time_diff;
 	//! This is a set of serial numbers that we have already seen, its used to find lidarId
 	std::set<std::string> m_serialNumbers;