Livox: Add new loadable libs for Protocolv1 and gen1 lidars

We've added two new libs: * commonLibs/livoxProto1 * senseApis/livoxGen1 They currently get to the point of detecting my Livox Avia on the network over UDP. This was really easy to get done in one night using boost::asio and Cursor, honestly.
2025-09-05 00:08:25 -04:00
parent f00e1c7cf1
commit 6eb6fa1eb0
13 changed files with 771 additions and 0 deletions
@@ -0,0 +1,175 @@
+#ifndef LIVOXPROTO1_PROTOCOL_H
+#define LIVOXPROTO1_PROTOCOL_H
+
+#include <vector>
+#include <string>
+#include <memory>
+#include <sstream>
+#include <atomic>
+#include <user/senseApiDesc.h>
+
+namespace livoxProto1 {
+namespace comms {
+
+// Endianness detection
+namespace endian {
+	inline bool isLittleEndian() {
+		union {
+			uint32_t i;
+			char c[4];
+		} test = {0x01020304};
+		return test.c[0] == 4;
+	}
+}
+
+/**		EXPLANATION:
+ * Device types as defined in the Livox protocol specification
+ */
+enum class DeviceType : uint8_t {
+	Hub = 0,
+	Mid40 = 1,
+	Tele15 = 2,
+	Horizon = 3,
+	Mid70 = 6,
+	Avia = 7
+};
+
+/**		EXPLANATION:
+ * Protocol frame header structure.
+ * All multi-byte fields are in little-endian format as per protocol spec.
+ */
+struct Header
+{
+	uint8_t sof;           // 0: Start of Frame (0xAA)
+	uint8_t version;       // 1: Protocol Version (1)
+	uint16_t length;       // 2-3: Frame Length (little-endian)
+	uint8_t cmd_type;      // 4: Command Type (0x02 = MSG for broadcast)
+	uint16_t seq_num;      // 5-6: Sequence Number (little-endian)
+	uint16_t crc_16;       // 7-8: Header Checksum (little-endian)
+
+	void swapToHostEndianness();
+	bool sanityCheck() const;
+} __attribute__((packed));
+
+/**		EXPLANATION:
+ * Protocol frame footer structure.
+ * All multi-byte fields are in little-endian format as per protocol spec.
+ */
+struct Footer
+{
+	uint32_t crc_32;       // 0-3: Whole Frame Checksum (little-endian)
+
+	void swapToHostEndianness();
+	bool sanityCheck() const;
+} __attribute__((packed));
+
+/**		EXPLANATION:
+ * Complete wire format for Livox broadcast messages.
+ * All multi-byte fields are in little-endian format as per protocol spec.
+ */
+struct BroadcastMessage
+{
+	Header header;         // 0-8: Protocol frame header
+	uint8_t cmd_set;       // 9: Command Set (0x00 = General)
+	uint8_t cmd_id;        // 10: Command ID (0x00 = Broadcast)
+	uint8_t broadcast_code[16]; // 11-26: Device Broadcast Code (null-terminated string)
+	uint8_t dev_type;      // 27: Device Type
+	uint16_t reserved;     // 28-29: Reserved (little-endian)
+	Footer footer;         // 30-33: Protocol frame footer
+
+	void swapToHostEndianness();
+	bool sanityCheck() const;
+} __attribute__((packed));
+
+/**		EXPLANATION:
+ * This class represents a discovered device. It is used to store the
+ * device identifier and IP address of a discovered device.
+ */
+class DiscoveredDevice
+{
+public:
+	DiscoveredDevice(
+		const std::string &deviceIdentifier,
+		DeviceType deviceType,
+		const std::string &ipAddr);
+
+	// "Conversion" constructor from BroadcastMessage
+	DiscoveredDevice(const BroadcastMessage &msg, const std::string &ipAddr);
+
+	~DiscoveredDevice() = default;
+
+	bool operator==(const DiscoveredDevice &other) const
+		{ return deviceIdentifier == other.deviceIdentifier; }
+
+	std::string stringify(void) const;
+	std::string getDeviceTypeName(void) const;
+
+public:
+	std::string deviceIdentifier;
+	DeviceType deviceType;
+	std::string ipAddr;
+};
+
+/**		EXPLANATION:
+ * This class merely listens for UDP bcast dgrams on the designated listening
+ * port. It then builds a list of client device IP addrs that it has heard from.
+ * It doesn't connect to them or signal any events to the rest of the lib,
+ * except in the case that a device which the lib is using has gone away.
+ *
+ * Other than that, its role is to tell the lib which devices are available
+ * on the network.
+ */
+#define UDP_BCAST_MSG_BUFFER_NBYTES		(1024)
+
+class BroadcastListener
+{
+public:
+	BroadcastListener(
+		const std::shared_ptr<smo::ComponentThread>& componentThread,
+		uint16_t listeningPort=55000, uint16_t connectPort=65000);
+
+	~BroadcastListener() = default;
+
+	typedef void (DeviceGoneAwayCbFn)(const DiscoveredDevice &device);
+	void setDeviceGoneAwayCb(DeviceGoneAwayCbFn *cb)
+		{ deviceGoneAwayCb = cb; }
+
+	bool deviceExists(const std::string &deviceIdentifier) const
+		{ return getDevice(deviceIdentifier) != nullptr; }
+
+	std::shared_ptr<DiscoveredDevice>
+	getDevice(const std::string &deviceIdentifier) const;
+
+	void start(void);
+	void stop(void);
+
+	void broadcastMsgInd(
+		const boost::system::error_code& ec, std::size_t bytes_received);
+
+private:
+	void startReceive(void);
+
+private:
+	std::shared_ptr<smo::ComponentThread> componentThread;
+	/**		EXPLANATION:
+	 * The Livox proto says that client devices will spam broadcast UDP
+	 * dgrams to us on the listening port. We can then use the source IP from
+	 * the bcast dgram to figure out the client device's IP addr. Then we
+	 * should send a connect dgram to the connect port. This will tell the
+	 * client device our IP addr.
+	 */
+	uint16_t listeningPort, connectPort;
+	DeviceGoneAwayCbFn *deviceGoneAwayCb;
+	std::vector<std::shared_ptr<DiscoveredDevice>> discoveredDevices;
+
+	boost::asio::ip::udp::socket socket;
+	boost::asio::ip::udp::endpoint listeningEndpoint, senderEndpoint;
+	std::atomic<bool> isListening;
+
+	uint8_t bcastMsgRecvBuffer[UDP_BCAST_MSG_BUFFER_NBYTES];
+};
+
+} // namespace comms
+} // namespace livoxProto1
+
+#endif // LIVOXPROTO1_PROTOCOL_H