feat: 添加 ahrs 和 excavator-bucket-position 模块

libs/ahrs/build.gradle.kts

@@ -0,0 +1,7 @@
+plugins {
+    kotlin("jvm")
+}
+
+repositories {
+    mavenCentral()
+}

libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401ACanConfigurationData.kt

@@ -0,0 +1,159 @@
+package com.icegps.ahrs
+
+/**
+ * @author linmiao
+ * @date 2025/2/6
+ */
+class AHRS401ACanConfigurationData {
+
+    // 波特率枚举
+    enum class BaudRate(val value: Int) {
+        BAUD_250K(250),
+        BAUD_500K(500),
+        BAUD_1000K(1000);
+    }
+
+    // 输出频率枚举
+    enum class OutputFrequency(val value: Int) {
+        FREQ_1HZ(1),
+        FREQ_10HZ(10),
+        FREQ_50HZ(50),
+        FREQ_100HZ(100),
+        FREQ_200HZ(200);
+    }
+
+    // 横滚俯仰取反枚举
+    enum class RollPitchInvert(val value: Int) {
+        NO_INVERT(0x00),
+        ROLL_INVERT(0x01),
+        PITCH_INVERT(0x10),
+        BOTH_INVERT(0x11);
+    }
+
+    // 滤波器截止频率枚举
+    enum class FilterCutoffFrequency(val value: Int) {
+        FREQ_10(10),
+        FREQ_20(20),
+        FREQ_40(40),
+        FREQ_47(47);
+    }
+
+    companion object {
+        // 默认节点ID
+        private val DEFAULT_NODE_ID = 0x00
+
+        val queryBaudRate = intArrayOf(0x20, 0x21, 0x22, 0x23, 0x0A, 0x00, 0x00, 0x00)
+        val queryVersion = intArrayOf(0x10, 0x11, 0x12, 0x13, 0x00, 0x00, 0x00, 0x00)
+        val removeResistor = intArrayOf(0x10, 0x11, 0x12, 0x13, 0x01, 0x00, 0x00, 0x00)
+        val addResistor = intArrayOf(0x10, 0x11, 0x12, 0x13, 0x02, 0x00, 0x00, 0x00)
+        val queryFrequecy = intArrayOf(0x10, 0x11, 0x12, 0x13, 0x0A, 0xFF, 0x00, 0x00)
+        val queryRollPitchInvertStatusdata = intArrayOf(0x10, 0x11, 0x12, 0x13, 0x0A, 0xFF, 0x00, 0x00)
+        val queryFilterCutoffFrequency = intArrayOf(0x20, 0x21, 0x22, 0x0A, 0xFF, 0xFF, 0x00, 0x00)
+        val queryCoordinateSystem = intArrayOf(0x30, 0x31, 0x32, 0x0A, 0xFF, 0xFF, 0x00, 0x00)
+        val queryRemoveAttitudeAngle = intArrayOf(0x10, 0x11, 0x12, 0x0A, 0xFF, 0xFF, 0x00, 0x00)
+
+
+        /**
+         * 设置 CAN 传感器的波特率。
+         *
+         * @param baudRate 传入的波特率取值：250 (kbps)，500 (kbps)，1000 (kbps)。
+         *
+         */
+        fun setBaudRate(baudRate: BaudRate): IntArray {
+            val data = intArrayOf(0x20, 0x21, 0x22, 0x23, 0x23, 0x00, 0x00, 0x00)
+
+            when (baudRate) {
+                BaudRate.BAUD_250K -> data[4] = 0x01
+                BaudRate.BAUD_500K -> data[4] = 0x02
+                BaudRate.BAUD_1000K -> data[4] = 0x03
+            }
+
+            return data
+        }
+
+        /**
+         * 设置输出频率。
+         *
+         * @param frequency 输出频率取值：1HZ 10HZ 50HZ 100HZ 200HZ
+         *                  例如，传入 `1` 表示输出频率为 1 Hz。
+         */
+        fun setOutputFrequency(frequency: OutputFrequency): IntArray {
+            val data = intArrayOf(0x10, 0x11, 0x12, 0x13, 0x0A, 0x00, 0x00, 0x00)
+
+            when (frequency) {
+                OutputFrequency.FREQ_1HZ -> data[4] = 0x01
+                OutputFrequency.FREQ_10HZ -> data[4] = 0x02
+                OutputFrequency.FREQ_50HZ -> data[4] = 0x03
+                OutputFrequency.FREQ_100HZ -> data[4] = 0x04
+                OutputFrequency.FREQ_200HZ -> data[4] = 0x05
+            }
+
+            return data
+        }
+
+        /**
+         * 设置横滚俯仰取反命令。
+         * ID=0x61D, DATA=0x10 0x11 0x12 0x13 XXXX 0xFF ID_H ID_L
+         * @param rollPitchValue 横滚俯仰取反值：
+         *                       - 0x00 不取反
+         *                       - 0x01 横滚取反
+         *                       - 0x10 俯仰取反
+         *                       - 0x11 横滚和俯仰都取反
+         */
+        fun setRollPitchInvert(rollPitchValue: RollPitchInvert): IntArray {
+            return intArrayOf(0x10, 0x11, 0x12, 0x13, rollPitchValue.value, 0xFF, 0x00, 0x00)
+        }
+
+        /**
+         * 设置滤波器截止频率命令。
+         * ID=0x61E, DATA=0x20 0x21 0x22 0x23 XXXX 0xFF ID_H ID_L
+         * @param frequency 滤波器截止频率取值：10 (Hz) 20 (Hz) 40 (Hz) 47 (Hz)
+         *                  例如，传入 `10` 表示截止频率为 10 Hz。
+         */
+        fun setFilterCutoffFrequency(frequency: FilterCutoffFrequency): IntArray {
+            val commandValue = when (frequency) {
+                FilterCutoffFrequency.FREQ_10 -> 0x44
+                FilterCutoffFrequency.FREQ_20 -> 0x66
+                FilterCutoffFrequency.FREQ_40 -> 0xAA
+                FilterCutoffFrequency.FREQ_47 -> 0xBB
+            }
+
+            return intArrayOf(0x20, 0x21, 0x22, 0x23, commandValue, 0xFF, 0x00, 0x00)
+        }
+
+        /**
+         * 设置坐标系命令。
+         * ID=0x61F, DATA=0x30 0x31 0x32 0x33 XXXX 0xFF ID_H ID_L
+         * @param coordinate 坐标系统的值，例如：`0x01` 表示某一坐标系统。
+         */
+        fun setCoordinateSystem(coordinate: Int): IntArray {
+            val data = intArrayOf(0x30, 0x31, 0x32, 0x33, coordinate, 0xFF, 0x00, 0x00)
+            return data
+        }
+
+        /**
+         * 设置是否扣除姿态角命令。
+         * ID=0x620, DATA=0x10 0x11 0x12 0x13 XXXX 0xFF ID_H ID_L
+         * @param remove 是否扣除姿态角，`true` 表示扣除，`false` 表示不扣除。
+         */
+        fun setRemoveAttitudeAngle(remove: Boolean): IntArray {
+            val data = intArrayOf(0x10, 0x11, 0x12, 0x13, if (remove) 0x01 else 0x00, 0xFF, 0x00, 0x00)
+            return data
+        }
+
+        /**
+         * 设置节点ID命令。
+         * ID=0x61A, DATA=0x30 0x31 0x32 0x33 0xXX 0xXX 0x00 0x00
+         * @param highByte 节点 ID 的高字节。
+         * @param lowByte 节点 ID 的低字节。
+         * 示例：highByte 可以是 0x01, lowByte 可以是 0x00
+         * @return 返回设置节点 ID 的命令数据
+         */
+        fun setNodeId(highByte: Int, lowByte: Int): IntArray {
+            if (highByte < 0 || highByte > 0xFF || lowByte < 0 || lowByte > 0xFF) {
+                throw IllegalArgumentException("Invalid highByte or lowByte value")
+            }
+            return intArrayOf(0x30, 0x31, 0x32, 0x33, highByte, lowByte, 0x00, 0x00)
+        }
+    }
+}

libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401ACanDataParser.kt

@@ -0,0 +1,191 @@
+package com.icegps.ahrs
+
+import java.nio.ByteBuffer
+import java.nio.ByteOrder
+
+/**
+ * @author linmiao
+ * @date 2025/2/7
+ */
+object AHRS401ACanDataParser {
+
+    private const val TAG = "AHRS401ACanDataParser"
+
+    // 用来缓存接收到的数据
+    private val dataCache = mutableMapOf<String, LongArray>()
+    private var count = 0
+
+    // 新增的查询命令的返回结果
+    var baudRate: Int? = null
+    var version: String? = null
+    var outputFrequency: Int? = null
+    var rollPitchInvert: String? = null
+    var filterCutoffFrequency: Int? = null
+    var coordinateSystem: Int? = null
+    var attitudeAngle: String? = null
+
+    fun parseFrame(frame: AHRS401ACanFrame): AHRS401AParsedCanData? {
+        try {
+
+            val data = AHRS401AParsedCanData()
+            val strid = frame.id.toString(16)
+//            Log.d(TAG, "Frame ID: $strid")
+//            Log.d(TAG, "Frame Len: ${frame.length}")
+
+            when (strid) {
+
+                "518" -> { // 版本号的回应 (ID=0x518)
+                    val versionBytes = frame.data.sliceArray(0..3) // 版本号
+                    // 将字节拼接为一个十六进制字符串
+                    val versionHex = versionBytes.joinToString("") { String.format("%02X", it) }
+
+                    // 将十六进制版本号字符串转换为十进制
+                    version = versionHex.toLong(16).toInt().toString()
+                    println("查询命令的返回结果Received version: $version")
+                }
+
+                "519" -> { // 波特率的回应 (ID=0x519)
+                    baudRate = when (frame.data[0]) {
+                        0x01L -> 250
+                        0x02L -> 500
+                        0x03L -> 1000
+                        else -> 0
+                    }
+                    println("查询命令的返回结果Received baud rate response: $baudRate")
+                }
+
+                "51c" -> { // 输出频率的回应 (ID=0x51C)
+                    val frequency = frame.data[0]
+                    outputFrequency = when (frequency) {
+                        0x01L -> 1
+                        0x02L -> 10
+                        0x03L -> 50
+                        0x04L -> 100
+                        0x05L -> 200
+                        else -> 0
+                    }
+                    println("查询命令的返回结果Received output frequency: $outputFrequency")
+                }
+
+                "51d" -> { // 横滚俯仰取反的回应 (ID=0x51D)
+                    val invertStatus = frame.data[0]
+                    rollPitchInvert = when (invertStatus) {
+                        0x00L -> "No invert"
+                        0x01L -> "Roll invert"
+                        0x10L -> "Pitch invert"
+                        0x11L -> "Both invert"
+                        else -> "Unknown"
+                    }
+                    println("查询命令的返回结果Roll/Pitch invert status: $rollPitchInvert")
+                }
+
+                "51e" -> { // 滤波器截止频率的回应 (ID=0x51E)
+                    val cutoffFrequency = frame.data[0]
+                    filterCutoffFrequency = when (cutoffFrequency) {
+                        0x44L -> 10
+                        0x66L -> 20
+                        0xAAL -> 40
+                        0xBBL -> 47
+                        else -> 0
+                    }
+                    println("查询命令的返回结果Received filter cutoff frequency: $filterCutoffFrequency")
+                }
+
+                "51f" -> { // 坐标系的回应 (ID=0x51F)
+                    coordinateSystem = frame.data[0].toInt()
+                    println("查询命令的返回结果Received coordinate system: $coordinateSystem")
+                }
+
+                "520" -> { // 姿态角的回应 (ID=0x520)
+                    val removeAttitudeAngle = frame.data[0]
+                    attitudeAngle = if (removeAttitudeAngle == 0x01L) "Removed" else "Not removed"
+                    println("查询命令的返回结果Attitude angle removal status: $attitudeAngle")
+                }
+
+                else -> {
+                    // 将当前帧缓存起来
+                    dataCache[strid] = frame.data
+                }
+            }
+
+
+            // 判断是否收齐了完整的数据（65, 66, 67, 68, 69）
+            if ((dataCache.containsKey("65") && dataCache.containsKey("66")) || (dataCache.containsKey("6a") && dataCache.containsKey(
+                    "6b"
+                ))
+//            && dataCache.containsKey("67") && dataCache.containsKey("68") &&
+//            dataCache.containsKey("69")
+            ) {
+                if (dataCache.containsKey("65")) {
+
+                    // 开始解析数据
+                    // 解析 ROLL & PITCH
+                    data.roll = parseFloat(dataCache["65"]!!, 0)
+                    data.pitch = parseFloat(dataCache["65"]!!, 4)
+//                Log.e(TAG, "parseFrame: data.roll${data.roll}\n  data.pitch${data.pitch}",)
+
+                    // 解析 YAW & Gx
+                    data.yaw = parseFloat(dataCache["66"]!!, 0)
+                    data.gx = parseFloat(dataCache["66"]!!, 4)
+//                Log.e(TAG, "parseFrame: data.yaw${data.roll}\n  data.gx${data.pitch}",)
+
+                } else if (dataCache.containsKey("6a")) {
+                    // 开始解析数据
+                    // 解析 ROLL & PITCH
+                    data.roll = parseFloat(dataCache["6a"]!!, 0)
+                    data.pitch = parseFloat(dataCache["6a"]!!, 4)
+//                    Log.e(TAG, "parseFrame: data.roll${data.roll}\n  data.pitch${data.pitch}",)
+
+                    // 解析 YAW & Gx
+                    data.yaw = parseFloat(dataCache["6b"]!!, 0)
+                    data.gx = parseFloat(dataCache["6b"]!!, 4)
+//                    Log.e(TAG, "parseFrame: data.yaw${data.roll}\n  data.gx${data.pitch}",)
+                }
+
+
+                // 解析 Gy & Gz
+//            data.gy = parseFloat(dataCache["67"]!!, 0)
+//            data.gz = parseFloat(dataCache["67"]!!, 4)
+
+                // 解析 Ax & Ay
+//            data.ax = parseFloat(dataCache["68"]!!, 0)
+//            data.ay = parseFloat(dataCache["68"]!!, 4)
+
+                // 解析 Az & TEMP & INDEX
+//            data.az = parseFloat(dataCache["69"]!!, 0)
+//            data.temp = parseInt16(dataCache["69"]!!, 4)
+//            data.index = parseInt16(dataCache["69"]!!, 6)
+
+//            count++
+//                Log.d(TAG, "Received data count: $count")
+                // 清空缓存
+                dataCache.clear()
+
+                return data
+            }
+
+
+            // 如果没有收到完整的数据，返回 null
+            return null
+        } catch (e: Exception) {
+            return null
+        }
+    }
+
+
+    private fun parseFloat(data: LongArray, offset: Int): Float {
+        val bytes = ByteArray(4)
+        for (i in 0..3) {
+            bytes[i] = data[offset + i].toByte()
+        }
+        return ByteBuffer.wrap(bytes).order(ByteOrder.LITTLE_ENDIAN).getFloat()
+    }
+
+    private fun parseInt16(data: LongArray, offset: Int): Int {
+        val bytes = ByteArray(2)
+        for (i in 0..1) {
+            bytes[i] = data[offset + i].toByte()
+        }
+        return ByteBuffer.wrap(bytes).order(ByteOrder.LITTLE_ENDIAN).short.toInt()
+    }
+}

libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401ACanFrame.kt

@@ -0,0 +1,22 @@
+package com.icegps.ahrs
+
+import java.util.*
+
+/**
+ * @author linmiao
+ * @date 2025/2/7
+ */
+class AHRS401ACanFrame(revdata: LongArray) {
+    val id: Long = revdata[0]
+    val eff: Long = revdata[1]  // 扩展帧标志
+    val rtr: Long = revdata[2]  // 远程帧标志
+    val length: Int = revdata[3].toInt()
+    val data: LongArray = Arrays.copyOfRange(revdata, 4, 4 + length)
+
+    init {
+        require(revdata.size >= 4) { "revdata must contain at least 4 elements" }
+        val length = revdata[3].toInt()
+        require(length >= 0 && length <= (revdata.size - 4)) { "Invalid data length or insufficient revdata size" }
+    }
+
+}

libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401ACanManager.kt

@@ -0,0 +1,234 @@
+package com.icegps.ahrs
+
+import java.util.*
+import java.util.concurrent.atomic.AtomicBoolean
+import java.util.concurrent.atomic.AtomicLong
+
+/**
+ * @author linmiao
+ * @date 2025/2/7
+ */
+class AHRS401ACanManager(val canDevice: android_socketcan, private val interfaceName: String) {
+    private val TAG = "AHRS401ACanManager"
+
+    // private val canDevice = android_socketcan()
+    private val listeners = mutableListOf<OnCanDataListener>()
+    private val isRunning = AtomicBoolean(false)
+    private var receiveThread: Thread? = null
+
+    // 记录上次发送配置命令的时间
+    private val lastConfigCommandTime = AtomicLong(0)
+
+    interface OnCanDataListener {
+        fun onCanDataReceived(data: AHRS401AParsedCanData)
+    }
+
+    init {
+        canDevice.fd = canDevice.socketcanOpen(interfaceName)
+        println("初始化: $interfaceName")
+    }
+
+    fun addListener(listener: OnCanDataListener) {
+        if (!listeners.contains(listener)) {
+            listeners.add(listener)
+        }
+    }
+
+    fun removeListener(listener: OnCanDataListener) {
+        listeners.remove(listener)
+    }
+
+    fun startListening() {
+        if (isRunning.get()) return
+
+        isRunning.set(true)
+        receiveThread = Thread {
+            var ret = LongArray(12)
+            while (isRunning.get()) {
+                ret = canDevice.socketcanRead(canDevice.fd)
+                val can0id = ret[0]
+                val can0eff = ret[1]
+                val can0rtr = ret[2]
+                val can0len = ret[3]
+                // 确保 can0len 不会超出 ret 数组的大小
+                val length = can0len.toInt()
+                val toIndex = 4 + length
+                val can0data = if (toIndex <= ret.size) {
+                    ret.copyOfRange(4, toIndex)
+                } else {
+                    println("Invalid can0len: $can0len, ret size: ${ret.size}")
+                    LongArray(0)  // 或者根据需要处理错误情况
+                }
+
+                // 打印日志
+//                println("CAN ID: ${java.lang.Long.toHexString(can0id)}, Eff: $can0eff, RTR: $can0rtr, Length: $can0len, Data: ${Arrays.toString(can0data)}")
+
+                // 创建 CanFrame 对象并处理数据
+                if (can0data.isNotEmpty()) {
+                    val frame = AHRS401ACanFrame(ret)
+                    val parsedData = AHRS401ACanDataParser.parseFrame(frame)
+                    // 如果解析到了完整的数据，则通知监听者
+                    if (parsedData != null) {
+                        notifyListeners(parsedData)
+                    }
+                }
+            }
+        }
+        receiveThread?.start()
+    }
+
+    fun stopListening() {
+        isRunning.set(false)
+        receiveThread?.let {
+            try {
+                it.join()
+            } catch (e: InterruptedException) {
+                println("Error stopping receive thread")
+            }
+        }
+    }
+
+    private fun notifyListeners(data: AHRS401AParsedCanData) {
+        listeners.forEach { it.onCanDataReceived(data) }
+    }
+
+    fun sendCanData(id: Long, data: IntArray, isConfigCommand: Boolean) {
+        val currentTime = System.currentTimeMillis()
+
+        // 如果是配置命令，检查时间间隔
+        if (isConfigCommand) {
+            val lastTime = lastConfigCommandTime.get()
+            if (currentTime - lastTime < 3000) {
+                println("配置命令发送太频繁，等待 3 秒再发送")
+                return
+            }
+            lastConfigCommandTime.set(currentTime)
+        }
+
+        // 发送数据
+        if (canDevice.fd >= 0) {
+            println("Sending data: ID=${java.lang.Long.toHexString(id)}, Data=${Arrays.toString(data)}")
+            canDevice.socketcanWrite(canDevice.fd, id, 0, 0, data.size, data)
+        } else {
+            println("CAN device is not initialized or opened")
+        }
+    }
+
+    fun queryVersion() {
+        sendCanData(0x618, AHRS401ACanConfigurationData.queryVersion, false)
+    }
+
+    fun queryBaudRate() {
+        sendCanData(0x619, AHRS401ACanConfigurationData.queryBaudRate, false)
+    }
+
+    fun setBaudRate(baudRate: AHRS401ACanConfigurationData.BaudRate) {
+        sendCanData(0x619, AHRS401ACanConfigurationData.setBaudRate(baudRate), true)
+    }
+
+    fun addTerminalResistor() {
+        sendCanData(0x61B, AHRS401ACanConfigurationData.addResistor, true)
+    }
+
+    fun removeTerminalResistor() {
+        sendCanData(0x61B, AHRS401ACanConfigurationData.removeResistor, true)
+    }
+
+    fun queryOutputFrequency() {
+        sendCanData(0x61C, AHRS401ACanConfigurationData.queryFrequecy, false)
+    }
+
+    fun setOutputFrequency(frequency: AHRS401ACanConfigurationData.OutputFrequency) {
+        sendCanData(0x61C, AHRS401ACanConfigurationData.setOutputFrequency(frequency), true)
+    }
+
+    fun queryRollPitchInvertStatus() {
+        sendCanData(0x61D, AHRS401ACanConfigurationData.queryRollPitchInvertStatusdata, false)
+    }
+
+    fun setRollPitchInvert(rollPitchValue: AHRS401ACanConfigurationData.RollPitchInvert) {
+        sendCanData(0x61D, AHRS401ACanConfigurationData.setRollPitchInvert(rollPitchValue), true)
+    }
+
+    fun queryFilterCutoffFrequency() {
+        sendCanData(0x61E, AHRS401ACanConfigurationData.queryFilterCutoffFrequency, false)
+    }
+
+    fun setFilterCutoffFrequency(frequency: AHRS401ACanConfigurationData.FilterCutoffFrequency) {
+        sendCanData(0x61E, AHRS401ACanConfigurationData.setFilterCutoffFrequency(frequency), true)
+    }
+
+    fun queryCoordinateSystem() {
+        sendCanData(0x61F, AHRS401ACanConfigurationData.queryCoordinateSystem, false)
+    }
+
+    fun setCoordinateSystem(coordinate: Int) {
+        sendCanData(0x61F, AHRS401ACanConfigurationData.setCoordinateSystem(coordinate), true)
+    }
+
+    fun queryRemoveAttitudeAngle() {
+        sendCanData(0x620, AHRS401ACanConfigurationData.queryRemoveAttitudeAngle, false)
+    }
+
+    fun setRemoveAttitudeAngle(remove: Boolean) {
+        sendCanData(0x620, AHRS401ACanConfigurationData.setRemoveAttitudeAngle(remove), true)
+    }
+
+
+    fun configCanShell(baudRate: String): Boolean {
+        val isPermission = ShellUtils.checkRootPermission()
+        println("result的结果: $isPermission")
+        if (isPermission) {
+            try {
+                Thread.sleep(10000)
+            } catch (e: InterruptedException) {
+                println("Sleep interrupted")
+            }
+            val ret = ShellUtils.execCommand("ip link set down can0", true)
+            println("result: ${ret.result}")
+
+            val baudRateCommand = "ip link set can0 type can bitrate $baudRate sample-point 0.875 restart-ms 100"
+            val baudRateResult = ShellUtils.execCommand(baudRateCommand, true)
+            if (baudRateResult.result == 0) {
+                println("can0 set BaudRate [$baudRate] Success!")
+            }
+
+            val upCommand = "ip link set up can0 mtu 16"
+            val upResult = ShellUtils.execCommand(upCommand, true)
+            if (upResult.result == 0) {
+                println("can0 up Success!")
+            }
+
+            val txQueueLenCommand = "ifconfig can0 txqueuelen 1000"
+            val txQueueLenResult = ShellUtils.execCommand(txQueueLenCommand, true)
+            if (txQueueLenResult.result == 0) {
+                println("can0 txqueuelen Success!")
+            }
+
+            val downCan1Command = "ip link set down can1"
+            val downCan1Result = ShellUtils.execCommand(downCan1Command, true)
+            println("result can1: ${downCan1Result.result}")
+
+            val baudRateCan1Command = "ip link set can1 type can bitrate $baudRate sample-point 0.875 restart-ms 100"
+            val baudRateCan1Result = ShellUtils.execCommand(baudRateCan1Command, true)
+            if (baudRateCan1Result.result == 0) {
+                println("can1 set BaudRate [$baudRate] Success!")
+            }
+
+            val upCan1Command = "ip link set up can1 mtu 16"
+            val upCan1Result = ShellUtils.execCommand(upCan1Command, true)
+            if (upCan1Result.result == 0) {
+                println("can1 up Success!")
+            }
+
+            val txQueueLenCan1Command = "ifconfig can1 txqueuelen 1000"
+            val txQueueLenCan1Result = ShellUtils.execCommand(txQueueLenCan1Command, true)
+            if (txQueueLenCan1Result.result == 0) {
+                println("can1 txqueuelen Success!")
+            }
+        } else {
+            println("shell root is [$isPermission] failed!")
+        }
+        return isPermission
+    }
+}

libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401AParsedCanData.kt

@@ -0,0 +1,30 @@
+package com.icegps.ahrs
+
+/**
+ * @author linmiao
+ * @date 2025/2/7
+ */
+class AHRS401AParsedCanData {
+    var roll: Float = 0f
+    var pitch: Float = 0f
+    var yaw: Float = 0f
+    var gx: Float = 0f
+    var gy: Float = 0f
+    var gz: Float = 0f
+    var ax: Float = 0f
+    var ay: Float = 0f
+    var az: Float = 0f
+    var temp: Int = 0
+    var index: Int = 0
+    override fun equals(other: Any?): Boolean {
+        return super.equals(other)
+    }
+
+    override fun hashCode(): Int {
+        return super.hashCode()
+    }
+
+    override fun toString(): String {
+        return super.toString()
+    }
+}

libs/ahrs/src/main/kotlin/com/icegps/ahrs/ShellUtils.kt

@@ -0,0 +1,93 @@
+package com.icegps.ahrs
+
+import java.io.BufferedReader
+import java.io.DataOutputStream
+import java.io.IOException
+import java.io.InputStreamReader
+
+/**
+ * Shell 工具类
+ *
+ * @author lm
+ * @date 2024/12/26
+ */
+object ShellUtils {
+
+    private const val COMMAND_SU = "/system/xbin/su"
+    private const val COMMAND_SH = "sh"
+    private const val COMMAND_EXIT = "exit\n"
+    private const val COMMAND_LINE_END = "\n"
+
+    fun checkRootPermission(): Boolean {
+        return execCommand("echo root", true, false).result == 0
+    }
+
+    fun execCommand(command: String, isRoot: Boolean = false, isNeedResultMsg: Boolean = true): CommandResult {
+        return execCommand(arrayOf(command), isRoot, isNeedResultMsg)
+    }
+
+    fun execCommand(commands: List<String>?, isRoot: Boolean = false, isNeedResultMsg: Boolean = true): CommandResult {
+        return execCommand(commands?.toTypedArray(), isRoot, isNeedResultMsg)
+    }
+
+    fun execCommand(commands: Array<String>?, isRoot: Boolean, isNeedResultMsg: Boolean): CommandResult {
+        var result = -1
+        if (commands.isNullOrEmpty()) {
+            return CommandResult(result, null, null)
+        }
+
+        var process: Process? = null
+        var successResult: BufferedReader? = null
+        var errorResult: BufferedReader? = null
+        var successMsg: StringBuilder? = null
+        var errorMsg: StringBuilder? = null
+
+        var os: DataOutputStream? = null
+        try {
+            process = Runtime.getRuntime().exec(if (isRoot) COMMAND_SU else COMMAND_SH)
+            os = DataOutputStream(process.outputStream)
+            for (command in commands) {
+                if (command.isEmpty()) continue
+                os.write(command.toByteArray())
+                os.writeBytes(COMMAND_LINE_END)
+                os.flush()
+            }
+            os.writeBytes(COMMAND_EXIT)
+            os.flush()
+
+            result = process.waitFor()
+            if (isNeedResultMsg) {
+                successMsg = StringBuilder()
+                errorMsg = StringBuilder()
+                successResult = BufferedReader(InputStreamReader(process.inputStream))
+                errorResult = BufferedReader(InputStreamReader(process.errorStream))
+                var s: String?
+                while (successResult.readLine().also { s = it } != null) {
+                    successMsg.append(s)
+                }
+                while (errorResult.readLine().also { s = it } != null) {
+                    errorMsg.append(s)
+                }
+            }
+        } catch (e: Exception) {
+            e.printStackTrace()
+        } finally {
+            try {
+                os?.close()
+                successResult?.close()
+                errorResult?.close()
+            } catch (e: IOException) {
+                e.printStackTrace()
+            }
+            process?.destroy()
+        }
+        return CommandResult(result, successMsg?.toString(), errorMsg?.toString())
+    }
+
+    data class CommandResult(
+        val result: Int,
+        val successMsg: String?,
+        val errorMsg: String?
+    )
+
+}

libs/ahrs/src/main/kotlin/com/icegps/ahrs/android_socketcan.java

@@ -0,0 +1,16 @@
+package com.icegps.ahrs;
+
+/**
+ * @author linmiao
+ * @date 2025/1/9
+ */
+public class android_socketcan {
+    static {
+        System.loadLibrary("android_socketcan");
+    }
+    public int fd;
+    public native int socketcanOpen(String canx);   //return fd
+    public native int socketcanClose(int fd);       //return 0 is success
+    public native int socketcanWrite(int fd, long canid, long eff, long rtr, int len, int[] data);
+    public native long[] socketcanRead(int fd);
+}

libs/excavator-bucket-position/build.gradle.kts

@@ -0,0 +1,11 @@
+plugins {
+    kotlin("jvm")
+}
+
+repositories {
+    mavenCentral()
+}
+
+dependencies {
+    implementation(project(":libs:ahrs"))
+}

libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/BlhToEnu.kt

@@ -0,0 +1,263 @@
+package com.icegps.excavator.model
+
+import kotlin.math.*
+
+/**
+ * BLH -> ENU
+ *
+ * @author lm
+ * @date 2025/3/12
+ * @link https://gist.github.com/komasaru/6ce0634475923ddac597f868288c54e9
+ */
+class BlhToEnu {
+
+    companion object {
+        private const val PI_180 = Math.PI / 180.0
+
+        // WGS84 坐标参数
+        private const val A = 6378137.0 // a（地球椭球体长半径（赤道面平均半径））
+        private const val ONE_F = 298.257223563 // 1 / f（地球椭球体扁平率 = (a - b) / a）
+        private val B = A * (1.0 - 1.0 / ONE_F) // b（地球椭球体短半径）
+        private val E2 = (1.0 / ONE_F) * (2 - (1.0 / ONE_F))
+
+        // e^2 = 2 * f - f * f = (a^2 - b^2) / a^2
+        private val ED2 = E2 * A * A / (B * B) // e'^2 = (a^2 - b^2) / b^2
+    }
+
+    private var originLat: Double = 0.0
+    private var originLon: Double = 0.0
+    private var originHeight: Double = 0.0
+    private var isOriginSet: Boolean = false
+
+    fun resetEnuBenchmarkPoint() {
+        isOriginSet = false
+    }
+
+    fun wgs84ToEnu(lat: Double, lon: Double, height: Double = 0.0): DoubleArray {
+        if (!isOriginSet) {
+            originLat = lat
+            originLon = lon
+            originHeight = height
+            isOriginSet = true
+            return doubleArrayOf(0.0, 0.0, 0.0)
+        }
+
+        val enu = blh2enu(originLat, originLon, originHeight, lat, lon, height)
+        var az = atan2(enu[0], enu[1]) * 180.0 / Math.PI
+        if (az < 0.0) {
+            az += 360.0
+        }
+        val el = atan2(
+            enu[2],
+            sqrt(enu[0] * enu[0] + enu[1] * enu[1])
+        ) * 180.0 / Math.PI
+        val dst = sqrt(enu.sumOf { it * it })
+
+//        println("--->")
+//        println(
+//            """
+//            ENU: E = ${enu[0].format(3)}m
+//                 N = ${enu[1].format(3)}m
+//                 U = ${enu[2].format(3)}m
+//            方位角 = ${az.format(3)}°
+//              仰角 = ${el.format(3)}°
+//              距离 = ${dst.format(3)}m
+//        """.trimIndent()
+//        )
+
+        return enu
+    }
+
+    fun enuToWgs84(e: Double, n: Double, u: Double): DoubleArray {
+        if (!isOriginSet) {
+            return doubleArrayOf(0.0, 0.0, 0.0)
+        }
+
+        val blh = enu2blh(originLat, originLon, originHeight, e, n, u)
+
+//        println("--->")
+//        println(
+//            """
+//            BLH: Beta = ${blh[0].format(8)}°
+//                 Lambda = ${blh[1].format(8)}°
+//                 Height = ${blh[2].format(3)}m
+//        """.trimIndent()
+//        )
+
+        return blh
+    }
+
+    private fun Double.format(digits: Int) = "%.${digits}f".format(this)
+
+    /**
+     * BLH -> ENU 转换(East, North, Up)
+     *
+     * @param bO 原点 Beta(纬度)
+     * @param lO 原点 Lambda(经度)
+     * @param hO 原点 Height(高度)
+     * @param b 目标点 Beta(纬度)
+     * @param l 目标点 Lambda(经度)
+     * @param h 目标点 Height(高度)
+     * @return ENU 坐标 [e, n, u]
+     */
+    private fun blh2enu(bO: Double, lO: Double, hO: Double, b: Double, l: Double, h: Double): DoubleArray {
+        val (xO, yO, zO) = blh2ecef(bO, lO, hO)
+        val (x, y, z) = blh2ecef(b, l, h)
+        val mat0 = matZ(90.0)
+        val mat1 = matY(90.0 - bO)
+        val mat2 = matZ(lO)
+        val mat = mulMat(mulMat(mat0, mat1), mat2)
+        return rotate(mat, doubleArrayOf(x - xO, y - yO, z - zO))
+    }
+
+    /**
+     * BLH -> ECEF 转换
+     *
+     * @param lat 纬度
+     * @param lon 经度
+     * @param ht 高度
+     * @return ECEF 坐标 [x, y, z]
+     */
+    private fun blh2ecef(lat: Double, lon: Double, ht: Double): DoubleArray {
+        val n = { x: Double -> A / sqrt(1.0 - E2 * sin(x * PI_180).pow(2)) }
+        val x = (n(lat) + ht) * cos(lat * PI_180) * cos(lon * PI_180)
+        val y = (n(lat) + ht) * cos(lat * PI_180) * sin(lon * PI_180)
+        val z = (n(lat) * (1.0 - E2) + ht) * sin(lat * PI_180)
+        return doubleArrayOf(x, y, z)
+    }
+
+    /**
+     * ENU -> BLH 转换
+     *
+     * @param e East 坐标
+     * @param n North 坐标
+     * @param u Up 坐标
+     * @return WGS84 坐标 [纬度, 经度, 高度]
+     */
+    private fun enu2blh(bO: Double, lO: Double, hO: Double, e: Double, n: Double, u: Double): DoubleArray {
+        val mat0 = matZ(-lO)
+        val mat1 = matY(-(90.0 - bO))
+        val mat2 = matZ(-90.0)
+        val mat = mulMat(mulMat(mat0, mat1), mat2)
+
+        val enu = doubleArrayOf(e, n, u)
+        val xyz = rotate(mat, enu)
+
+        val (xO, yO, zO) = blh2ecef(bO, lO, hO)
+        val x = xyz[0] + xO
+        val y = xyz[1] + yO
+        val z = xyz[2] + zO
+
+        return ecef2blh(x, y, z)
+    }
+
+    /**
+     * ECEF -> BLH 转换
+     * @param x ECEF X 坐标
+     * @param y ECEF Y 坐标
+     * @param z ECEF Z 坐标
+     * @return WGS84 坐标 [纬度, 经度, 高度]
+     */
+    private fun ecef2blh(x: Double, y: Double, z: Double): DoubleArray {
+        val p = sqrt(x * x + y * y)
+        val theta = atan2(z * A, p * B)
+        val sinTheta = sin(theta)
+        val cosTheta = cos(theta)
+
+        val lat = atan2(
+            z + ED2 * B * sinTheta.pow(3),
+            p - E2 * A * cosTheta.pow(3)
+        )
+        val lon = atan2(y, x)
+
+        val sinLat = sin(lat)
+        val n = A / sqrt(1.0 - E2 * sinLat * sinLat)
+        val h = p / cos(lat) - n
+
+        return doubleArrayOf(
+            lat * 180.0 / Math.PI,
+            lon * 180.0 / Math.PI,
+            h
+        )
+    }
+
+    /**
+     * 以 x 轴为轴的旋转矩阵
+     *
+     * @param ang 旋转角度(°)
+     * @return 旋转矩阵(3x3)
+     */
+    private fun matX(ang: Double): Array<DoubleArray> {
+        val a = ang * PI_180
+        val c = cos(a)
+        val s = sin(a)
+        return arrayOf(
+            doubleArrayOf(1.0, 0.0, 0.0),
+            doubleArrayOf(0.0, c, s),
+            doubleArrayOf(0.0, -s, c)
+        )
+    }
+
+    /**
+     * 以 y 轴为轴的旋转矩阵
+     *
+     * @param ang 旋转角度(°)
+     * @return 旋转矩阵(3x3)
+     */
+    private fun matY(ang: Double): Array<DoubleArray> {
+        val a = ang * PI_180
+        val c = cos(a)
+        val s = sin(a)
+        return arrayOf(
+            doubleArrayOf(c, 0.0, -s),
+            doubleArrayOf(0.0, 1.0, 0.0),
+            doubleArrayOf(s, 0.0, c)
+        )
+    }
+
+    /**
+     * 以 z 轴为轴的旋转矩阵
+     *
+     * @param ang 旋转角度(°)
+     * @return 旋转矩阵(3x3)
+     */
+    private fun matZ(ang: Double): Array<DoubleArray> {
+        val a = ang * PI_180
+        val c = cos(a)
+        val s = sin(a)
+        return arrayOf(
+            doubleArrayOf(c, s, 0.0),
+            doubleArrayOf(-s, c, 0.0),
+            doubleArrayOf(0.0, 0.0, 1.0)
+        )
+    }
+
+    /**
+     * 两个矩阵(3x3)的乘积
+     *
+     * @param matA 3x3 矩阵
+     * @param matB 3x3 矩阵
+     * @return 3x3 矩阵
+     */
+    private fun mulMat(matA: Array<DoubleArray>, matB: Array<DoubleArray>): Array<DoubleArray> {
+        return Array(3) { k ->
+            DoubleArray(3) { j ->
+                (0..2).sumOf { i -> matA[k][i] * matB[i][j] }
+            }
+        }
+    }
+
+    /**
+     * 点的旋转
+     *
+     * @param mat 3x3 旋转矩阵
+     * @param pt 旋转前坐标 [x, y, z]
+     * @return 旋转后坐标 [x, y, z]
+     */
+    private fun rotate(mat: Array<DoubleArray>, pt: DoubleArray): DoubleArray {
+        return DoubleArray(3) { j ->
+            (0..2).sumOf { i -> mat[j][i] * pt[i] }
+        }
+    }
+
+}

libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/ExcavatorConfig.kt

@@ -0,0 +1,62 @@
+package com.icegps.excavator.model
+
+/**
+ * @author xjh
+ * @date 2025/2/25
+ */
+
+// 添加一个全局变量来存储缓存的LCE值
+var cachedLCE: Double = 0.0
+
+/**
+ * 挖掘机参数配置类
+ */
+data class ExcavatorConfig(
+
+    /**
+     * RTK 相关距离参数
+     */
+    val rtkConfig: RTKConfiguration = RTKConfiguration(
+        mainRtkToBoomAxis = 1.0,  // 主 RTK 相位中心到支架底部的垂直距离
+        subRtkToBoomAxis = 1.0,   // 副 RTK 相位中心到支架底部的垂直距离
+        mainRtkFootToJoint1 = 1.0,   // 主 RTK 在大臂上的垂足到关节 1 的距离 （不必须）
+        subRtkFootToJoint2 = 1.0     // 副 RTK 在大臂上的垂足到关节 2 的距离
+    ),
+
+    /**
+     * 挖掘机尺寸信息
+     */
+    val excavatorDims: ExcavatorDimensions = ExcavatorDimensions(
+        boom = 1.000,           // 大臂长度（不必须）
+        stick = 1.0,          // 小臂长度
+        bucket = 1.0,         // 挖斗长度（不必要）
+        mainBoomWidth = 1.0,  // 主天线处大臂宽度
+        subBoomWidth = 1.0,   // 副天线处大臂宽度
+        mainAntennaBracketWidth = 1.0,        // 主天线支架宽度
+        subAntennaBracketWidth = 1.0,         // 副天线支架宽度
+        mainAntennaBottomToBoomAxis = 1.0,    // 主天线底面到大臂轴心的垂直距离
+        subAntennaBottomToBoomAxis = 1.0      // 副天线底面到大臂轴心的垂直距离
+    ),
+
+    /**
+     * 连杆长度参数
+     */
+    val linkageLengths: LinkageLengths = LinkageLengths(
+        LAB = 1.0,    // 惰连杆长度(AB)
+        LBC = 1.0,    // 斗连杆长度(BC)
+        LCD = 1.0,    // 前后轴套长度(CD)
+        LAD = 1.0,    // 伪轴至斗齿长度(AD)
+        LAE = 1.0,    // 伪轴至斗齿距离(AE)
+        LCE = cachedLCE,      // (CE)
+        LDE = 1.0,    // 斗轴至斗齿距离(DE)
+        W = 1.0       // 铲斗宽度(W)
+    ),
+
+    // 安装模式
+    val antennaInstallationMode: AntennaInstallationMode = AntennaInstallationMode.LEFT,
+    val sensorInstallationMode: SensorInstallationMode = SensorInstallationMode.CUSTOM,
+
+    // 传感器偏移量
+    val stickOffsetEulerAngles: EulerAngles = EulerAngles(0.0, 0.0, 0.0),
+    val idleConnectingRodOffsetEulerAngles: EulerAngles = EulerAngles(0.0, 0.0, 0.0)
+)

libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/ExcavatorPositionCalculator.kt

@@ -0,0 +1,603 @@
+package com.icegps.excavator.model
+
+import java.util.Locale
+import kotlin.math.PI
+import kotlin.math.abs
+import kotlin.math.acos
+import kotlin.math.atan2
+import kotlin.math.cos
+import kotlin.math.min
+import kotlin.math.pow
+import kotlin.math.sin
+import kotlin.math.sqrt
+
+/**
+ * 3D 向量类，用于表示位置和方向
+ */
+data class Vector3(val x: Double, val y: Double, val z: Double) {
+    operator fun plus(other: Vector3) = Vector3(x + other.x, y + other.y, z + other.z)
+    operator fun minus(other: Vector3) = Vector3(x - other.x, y - other.y, z - other.z)
+    operator fun times(scalar: Double) = Vector3(x * scalar, y * scalar, z * scalar)
+    operator fun div(scalar: Double) = Vector3(x / scalar, y / scalar, z / scalar)
+    operator fun unaryMinus() = Vector3(-x, -y, -z)
+
+    fun norm(): Double = sqrt(x * x + y * y + z * z)
+    fun normalize(): Vector3 = this / norm()
+    fun dot(other: Vector3): Double = x * other.x + y * other.y + z * other.z
+    fun cross(other: Vector3): Vector3 = Vector3(
+        y * other.z - z * other.y,
+        z * other.x - x * other.z,
+        x * other.y - y * other.x
+    )
+
+    fun distanceTo(other: Vector3): Double = (this - other).magnitude()
+    fun magnitude(): Double = sqrt(x * x + y * y + z * z)
+
+    companion object {
+        val ZERO = Vector3(0.0, 0.0, 0.0)
+        val UP = Vector3(0.0, 0.0, 1.0)
+    }
+}
+
+/**
+ * 天线安装方式
+ */
+enum class AntennaInstallationMode {
+    LEFT, // 左侧安装
+    RIGHT // 右侧安装
+}
+
+/**
+ * 传感器安装方式
+ */
+enum class SensorInstallationMode {
+    CUSTOM,    // 自定义安装方式
+    VERTICAL,  // 垂直安装（正装）：传感器的 Z 轴与机体 Z 轴平行
+    HORIZONTAL // 水平安装（侧装）：传感器的 Y 轴与机体 Z 轴平行
+}
+
+/**
+ * E 点计算方式
+ */
+enum class ECalculationMethod {
+    USE_AE, // 使用 AE 方式计算
+    USE_CE, // 使用 CE 方式计算
+}
+
+/**
+ * RTK 相关距离参数
+ */
+data class RTKConfiguration(
+    val mainRtkToBoomAxis: Double,   // 主 RTK 相位中心到支架底部的垂直距离
+    val subRtkToBoomAxis: Double,    // 副 RTK 相位中心到支架底部的垂直距离
+    val mainRtkFootToJoint1: Double, // 主 RTK 在大臂上的垂足到关节 1 的距离
+    val subRtkFootToJoint2: Double   // 副 RTK 在大臂上的垂足到关节 2 的距离
+)
+
+/**
+ * 挖掘机尺寸信息
+ */
+data class ExcavatorDimensions(
+    val boom: Double,                        // 大臂长度
+    val stick: Double,                       // 小臂长度
+    val bucket: Double,                      // 铲斗长度
+    val mainBoomWidth: Double,               // 主天线处大臂宽度
+    val subBoomWidth: Double,                // 副天线处大臂宽度
+    val mainAntennaBracketWidth: Double,     // 主天线支架宽度
+    val subAntennaBracketWidth: Double,      // 副天线支架宽度
+    val mainAntennaBottomToBoomAxis: Double, // 主天线底面到大小臂轴心的垂直距离
+    val subAntennaBottomToBoomAxis: Double   // 副天线底面到大小臂轴心的垂直距离
+)
+
+/**
+ * 连杆长度参数
+ */
+data class LinkageLengths(
+    val LAD: Double, // A 点到 D 点的距离
+    val LAB: Double, // A 点到 B 点的距离
+    val LBC: Double, // B 点到 C 点的距离
+    val LCD: Double, // C 点到 D 点的距离
+    val LAE: Double, // A 点到 E 点的距离
+    val LCE: Double, // C 点到 E 点的距离
+    val LDE: Double, // D 点到 E 点的距离
+    val W: Double    // E1 到 E2 的距离（铲斗宽度）
+)
+
+/**
+ * 欧拉角
+ */
+data class EulerAngles(
+    val yaw: Double,   // 偏航角
+    val pitch: Double, // 俯仰角
+    val roll: Double   // 横滚角
+) {
+    fun toList(): List<Double> = listOf(yaw, pitch, roll)
+}
+
+/**
+ * 挖掘机关键点
+ */
+data class ExcavatorKeyPoints(
+    val joint1: Vector3, // 关节1（大臂起点位置）
+    val joint2: Vector3, // 关节2（大臂末端/小臂起点位置）
+    val joint3: Vector3, // 关节3（小臂末端/铲斗连接点位置）
+    val A: Vector3,      // A点
+    val B: Vector3,      // B点
+    val C: Vector3,      // C点
+    val D: Vector3,      // D点
+    val E: Vector3,      // 齿尖中心点
+    val E1: Vector3,     // 齿尖左侧点
+    val E2: Vector3      // 齿尖右侧点
+)
+
+/**
+ * 挖掘机位置计算器
+ *
+ * @author lm
+ * @date 2025/3/5
+ */
+class ExcavatorPositionCalculator {
+
+    /**
+     * 应用欧拉角偏移量
+     */
+    private fun applyOffsets(angles: EulerAngles, offset: EulerAngles): EulerAngles {
+        return EulerAngles(
+            yaw = angles.yaw - offset.yaw,
+            pitch = angles.pitch - offset.pitch,
+            roll = angles.roll - offset.roll
+        )
+    }
+
+    /**
+     * 计算挖掘机关键点位置
+     *
+     * 机体坐标系：
+     * - X 轴：指向机器前方
+     * - Y 轴：指向机器右侧
+     * - Z 轴：指向机器上方
+     *
+     * @param mainRtkPos 主 RTK 相位中心的 ENU 坐标
+     * @param subRtkPos 副 RTK 相位中心的 ENU 坐标
+     * @param rtkConfig RTK 相关配置参数
+     * @param excavatorDims 挖掘机尺寸参数
+     * @param linkageLengths 连杆长度参数
+     * @param stickAngleSensor 小臂传感器的欧拉角，单位为弧度
+     * @param idleConnectingRodAngleSensor 惰连杆传感器的欧拉角，单位为弧度
+     * @param antennaInstallationMode 天线安装模式
+     * @param sensorInstallationMode 传感器安装模式
+     * @param eCalculationMethod E 点计算方式
+     * @param stickOffsetEulerAngles 小臂传感器的欧拉角偏移量，单位为弧度
+     * @param idleConnectingRodOffsetEulerAngles 惰连杆传感器的欧拉角偏移量，单位为弧度
+     * @return ExcavatorKeyPoints 包含所有关键点坐标的结果对象
+     */
+    fun calculateExcavatorKeyPoints(
+        mainRtkPos: Vector3,
+        subRtkPos: Vector3,
+        rtkConfig: RTKConfiguration,
+        excavatorDims: ExcavatorDimensions,
+        linkageLengths: LinkageLengths,
+        stickAngleSensor: EulerAngles,
+        idleConnectingRodAngleSensor: EulerAngles,
+        antennaInstallationMode: AntennaInstallationMode = AntennaInstallationMode.LEFT,
+        sensorInstallationMode: SensorInstallationMode = SensorInstallationMode.VERTICAL,
+        eCalculationMethod: ECalculationMethod = ECalculationMethod.USE_CE,
+        stickOffsetEulerAngles: EulerAngles = EulerAngles(0.0, 0.0, 0.0),
+        idleConnectingRodOffsetEulerAngles: EulerAngles = EulerAngles(0.0, 0.0, 0.0)
+    ): ExcavatorKeyPoints {
+        // 应用偏移量
+        val stickAngleSensorCorrected = applyOffsets(stickAngleSensor, stickOffsetEulerAngles)
+        val idleConnectingRodAngleSensorCorrected = applyOffsets(idleConnectingRodAngleSensor, idleConnectingRodOffsetEulerAngles)
+
+        // 根据传感器安装模式选择正确的角度
+        val stickAngle = when (sensorInstallationMode) {
+            SensorInstallationMode.CUSTOM -> stickAngleSensorCorrected.roll
+            SensorInstallationMode.VERTICAL -> stickAngleSensorCorrected.pitch
+            SensorInstallationMode.HORIZONTAL -> stickAngleSensorCorrected.yaw
+        }
+        val idleConnectingRodAngle = when (sensorInstallationMode) {
+            SensorInstallationMode.CUSTOM -> idleConnectingRodAngleSensorCorrected.roll
+            SensorInstallationMode.VERTICAL -> idleConnectingRodAngleSensorCorrected.pitch
+            SensorInstallationMode.HORIZONTAL -> idleConnectingRodAngleSensorCorrected.yaw
+        }
+
+        // 计算天线方向向量
+        val antennaVector = subRtkPos - mainRtkPos
+        val antennaDirection = antennaVector.normalize()
+
+        // 计算垂直于天线方向的向量（水平方向）
+        val antennaPerpendicular = antennaDirection.cross(Vector3.UP).normalize()
+
+        // 计算垂直于天线方向且指向"上方"的向量（垂直方向）
+        val antennaUp = antennaPerpendicular.cross(antennaDirection).normalize()
+
+        // 根据天线安装模式确定偏移方向
+        val offsetDirection = when (antennaInstallationMode) {
+            AntennaInstallationMode.LEFT -> -antennaPerpendicular
+            AntennaInstallationMode.RIGHT -> antennaPerpendicular
+        }
+
+        // 计算天线到大臂中心线的偏移
+        val mainOffset = (excavatorDims.mainBoomWidth + excavatorDims.mainAntennaBracketWidth) / 2
+        val subOffset = (excavatorDims.subBoomWidth + excavatorDims.subAntennaBracketWidth) / 2
+
+        // 计算 RTK 到大臂中心线的垂足
+        val mainRtkFoot = mainRtkPos - antennaUp * (rtkConfig.mainRtkToBoomAxis + excavatorDims.mainAntennaBottomToBoomAxis) - offsetDirection * mainOffset
+        val subRtkFoot = subRtkPos - antennaUp * (rtkConfig.subRtkToBoomAxis + excavatorDims.subAntennaBottomToBoomAxis) - offsetDirection * subOffset
+
+        // 计算大臂实际方向向量
+        val boomVector = subRtkFoot - mainRtkFoot
+        val boomDirection = boomVector.normalize()
+
+        // 计算连接点1的位置（大臂起点）
+        val joint1Pos = mainRtkFoot + boomDirection * rtkConfig.mainRtkFootToJoint1
+
+        // 计算连接点2的位置（大臂末端/小臂起点）
+        val joint2Pos = subRtkFoot - boomDirection * rtkConfig.subRtkFootToJoint2
+
+        // 计算 yaw 角（大臂在水平面上的方向）
+        val boomYaw = atan2(boomDirection.y, boomDirection.x)
+
+        // 计算小臂的方向
+        val stickDirection = Vector3(
+            cos(boomYaw) * cos(stickAngle),
+            sin(boomYaw) * cos(stickAngle),
+            sin(stickAngle)
+        ).normalize()
+
+        // 确保小臂方向与大臂方向在同一平面
+        val boomNormal = boomDirection.cross(Vector3.UP)
+        val stickDirectionAdjusted = (stickDirection - boomNormal * stickDirection.dot(boomNormal)).normalize()
+
+        // 计算连接点3的位置（小臂末端 / 铲斗连接点）
+        val joint3Pos = joint2Pos + stickDirectionAdjusted * excavatorDims.stick
+
+        // 设置 D 点
+        val D = joint3Pos
+
+        // 计算 A 点坐标
+        val A = D - stickDirectionAdjusted * linkageLengths.LAD
+
+        // 计算 B 点坐标
+        val idleConnectingRodDirection = Vector3(
+            cos(boomYaw) * cos(idleConnectingRodAngle),
+            sin(boomYaw) * cos(idleConnectingRodAngle),
+            sin(idleConnectingRodAngle)
+        )
+        val B = A + idleConnectingRodDirection * linkageLengths.LAB
+
+        val planeNormal = boomDirection.cross(Vector3.UP).normalize()
+
+        // 使用球面交点方法计算 C 点坐标
+        val (candidateC1, candidateC2) = findIntersection3d(B, linkageLengths.LBC, D, linkageLengths.LCD, planeNormal)
+            .let { (c1, c2) -> Pair(c1 ?: D, c2 ?: D) }
+
+        // 选择正确的 C 点
+        val C = selectCorrectCPoint(A, B, D, candidateC1, candidateC2)
+
+        // 计算 E 点坐标
+        val (candidateE1, candidateE2) = when (eCalculationMethod) {
+            ECalculationMethod.USE_AE ->
+                findIntersection3d(A, linkageLengths.LAE, D, linkageLengths.LDE, planeNormal)
+                    .let { (e1, e2) -> Pair(e1 ?: D, e2 ?: D) }
+
+            ECalculationMethod.USE_CE ->
+                findIntersection3d(C, linkageLengths.LCE, D, linkageLengths.LDE, planeNormal)
+                    .let { (e1, e2) -> Pair(e1 ?: D, e2 ?: D) }
+        }
+
+        // 选择正确的 E 点（位于 AB 的对面）
+        val ABMidpoint = (A + B) * 0.5
+        val ABToD = D - ABMidpoint
+        val E = if ((candidateE1 - ABMidpoint).dot(ABToD) > (candidateE2 - ABMidpoint).dot(ABToD)) candidateE1 else candidateE2
+
+        // 计算 E1 和 E2 点坐标
+        val boomDirectionHorizontal = Vector3(
+            boomDirection.x,
+            boomDirection.y,
+            0.0
+        ).normalize()
+
+        // 计算挖掘机朝向的右侧向量
+        val rightVector = boomDirectionHorizontal.cross(Vector3.UP).normalize()
+
+        // 计算铲斗方向向量
+        val bucketDirection = (E - D).normalize()
+
+        // 计算铲斗左右方向向量
+        var bucketLeftRight = bucketDirection.cross(stickDirectionAdjusted).normalize()
+
+        // 确保 bucketLeftRight 与 rightVector 方向一致
+        if (bucketLeftRight.dot(rightVector) < 0) {
+            bucketLeftRight = -bucketLeftRight
+        }
+
+        // 计算 E1（左铲尖）和 E2（右铲尖）
+        val E1 = E - bucketLeftRight * (linkageLengths.W / 2)
+        val E2 = E + bucketLeftRight * (linkageLengths.W / 2)
+
+        return ExcavatorKeyPoints(
+            joint1 = joint1Pos,
+            joint2 = joint2Pos,
+            joint3 = joint3Pos,
+            A = A,
+            B = B,
+            C = C,
+            D = D,
+            E = E,
+            E1 = E1,
+            E2 = E2
+        )
+    }
+
+    /**
+     * 限制角度在 -55 到 55 度之间
+     */
+    private fun limitAngle(angle: Double): Double {
+        return angle.coerceIn(-55.0, 55.0)
+    }
+
+    /**
+     * 选择正确的 C 点，基于多个几何和运动学条件
+     *
+     * @param A 已知的四连杆点坐标
+     * @param B 已知的四连杆点坐标
+     * @param D 已知的四连杆点坐标
+     * @param C1 候选 C 点坐标
+     * @param C2 候选 C 点坐标
+     * @return 选中的 C 点坐标
+     */
+    private fun selectCorrectCPoint(A: Vector3, B: Vector3, D: Vector3, C1: Vector3, C2: Vector3): Vector3 {
+        fun scoreCPoint(C: Vector3): Double {
+            /**
+             * 对 C 点进行综合评分
+             */
+            if (!isCoplanar(A, B, C, D) || !isValidQuadrilateral(A, B, C, D)) {
+                return Double.NEGATIVE_INFINITY
+            }
+
+            val angleScore = 1.0 / (1.0 + abs(calculateAngleSum(A, B, C, D) - Math.PI))
+            val transmissionScore = 1.0 / (1.0 + calculateTransmissionAngle(A, B, C, D))
+            val singularScore = if (checkSingularPosition(A, B, C, D)) 0.0 else 1.0
+
+            return angleScore + transmissionScore + singularScore
+        }
+
+        val C1Score = scoreCPoint(C1)
+        val C2Score = scoreCPoint(C2)
+
+        return if (C1Score > C2Score) C1 else C2
+    }
+
+    /**
+     * 检查四点是否共面
+     */
+    private fun isCoplanar(A: Vector3, B: Vector3, C: Vector3, D: Vector3, tolerance: Double = 1e-6): Boolean {
+        return abs((D - A).dot((B - A).cross(C - A))) < tolerance
+    }
+
+    /**
+     * 检查是否为有效的四边形
+     */
+    private fun isValidQuadrilateral(A: Vector3, B: Vector3, C: Vector3, D: Vector3): Boolean {
+        val points = listOf(A, B, C, D)
+        if (points.any { p1 -> points.any { p2 -> p1 != p2 && p1.distanceTo(p2) < 1e-6 } }) {
+            return false  // 存在重合点
+        }
+
+        return !(lineIntersection(A, B, C, D) || lineIntersection(A, D, B, C))
+    }
+
+    /**
+     * 检查两条线段是否相交
+     */
+    private fun lineIntersection(p1: Vector3, p2: Vector3, p3: Vector3, p4: Vector3): Boolean {
+        val v1 = p2 - p1
+        val v2 = p4 - p3
+        val normal = v1.cross(v2)
+        if (normal.magnitude() < 1e-6) {
+            return false  // 平行或共线
+        }
+
+        val t = (p3 - p1).dot(v2.cross(normal)) / v1.dot(v2.cross(normal))
+        val u = (p1 - p3).dot(v1.cross(normal)) / v2.dot(v1.cross(normal))
+
+        return t in 0.0..1.0 && u in 0.0..1.0
+    }
+
+    /**
+     * 计算 ∠BAD 和 ∠BCD 的和
+     */
+    private fun calculateAngleSum(A: Vector3, B: Vector3, C: Vector3, D: Vector3): Double {
+        val BA = A - B
+        val BC = C - B
+        val DA = A - D
+        val DC = C - D
+        val angleBAD = acos(BA.dot(DA) / (BA.magnitude() * DA.magnitude()))
+        val angleBCD = acos(BC.dot(DC) / (BC.magnitude() * DC.magnitude()))
+        return angleBAD + angleBCD
+    }
+
+    /**
+     * 计算传动角，返回与 90 度的偏差
+     */
+    private fun calculateTransmissionAngle(A: Vector3, B: Vector3, C: Vector3, D: Vector3): Double {
+        val BA = A - B
+        val BC = C - B
+        val DC = C - D
+        val DA = A - D
+        val angle1 = acos(BA.dot(BC) / (BA.magnitude() * BC.magnitude()))
+        val angle2 = acos(DC.dot(DA) / (DC.magnitude() * DA.magnitude()))
+        return min(abs(Math.PI / 2 - angle1), abs(Math.PI / 2 - angle2))
+    }
+
+    /**
+     * 检查是否处于奇异位置
+     */
+    private fun checkSingularPosition(A: Vector3, B: Vector3, C: Vector3, D: Vector3, tolerance: Double = 1e-6): Boolean {
+        val BA = A - B
+        val BC = C - B
+        val DC = C - D
+        val DA = A - D
+        return (abs(BA.dot(BC) / (BA.magnitude() * BC.magnitude())) > 1 - tolerance ||
+                abs(DC.dot(DA) / (DC.magnitude() * DA.magnitude())) > 1 - tolerance)
+    }
+
+    /**
+     * 计算 3D 空间中两个球面在给定平面上的交点
+     *
+     * @param center1 第一个球的中心点坐标
+     * @param radius1 第一个球的半径
+     * @param center2 第二个球的中心点坐标
+     * @param radius2 第二个球的半径
+     * @param planeNormal 平面的法向量
+     * @return 两个交点的坐标，如果没有交点则返回 null, null
+     */
+    private fun findIntersection3d(center1: Vector3, radius1: Double, center2: Vector3, radius2: Double, planeNormal: Vector3): Pair<Vector3?, Vector3?> {
+        // 计算两个球心之间的向量
+        val centerVector = center2 - center1
+
+        // 计算中心向量在平面法线上的投影
+        val projection = planeNormal * centerVector.dot(planeNormal)
+
+        // 计算中心向量在平面内的分量
+        val centerVectorInPlane = centerVector - projection
+
+        // 计算平面内两圆心距离
+        val d = centerVectorInPlane.magnitude()
+
+        // 检查是否有交点，如果两圆心距离大于两半径之和，或小于两半径之差，则没有交点
+        if (d > radius1 + radius2 || d < abs(radius1 - radius2)) {
+            return Pair(null, null)
+        }
+
+        // 计算交点到 center1 的距离
+        val a = (radius1.pow(2) - radius2.pow(2) + d.pow(2)) / (2 * d)
+
+        // 计算交点到连接线的垂直距离
+        val h = sqrt(radius1.pow(2) - a.pow(2))
+
+        // 计算交点在连接线上的位置
+        val intersectionBase = center1 + centerVectorInPlane * (a / d)
+
+        // 计算垂直于连接线和平面法线的向量
+        val perpendicular = centerVectorInPlane.cross(planeNormal).normalize()
+
+        // 计算两个交点
+        val intersection1 = intersectionBase + perpendicular * h
+        val intersection2 = intersectionBase - perpendicular * h
+
+        return Pair(intersection1, intersection2)
+    }
+
+    /**
+     * 将角度转换为弧度
+     */
+    fun degreesToRadians(degrees: Double): Double = degrees * PI / 180.0
+
+    /**
+     * 将弧度转换为角度
+     */
+    fun radiansToDegrees(radians: Double): Double = radians * 180.0 / PI
+
+    /**
+     * 将 ±180 度系统转换为 0-360 度系统
+     */
+    fun convert180To360(angle: Double): Double {
+        return when {
+            angle in -90.0..90.0 -> 90.0 - angle
+            angle > 90.0 -> 450.0 - angle
+            else -> 90.0 - angle // angle < -90
+        }
+    }
+
+    /**
+     * 将 0-360 度系统转换为 ±180 度系统
+     */
+    fun convert360To180(angle: Double): Double {
+        return when {
+            angle in 0.0..180.0 -> 90.0 - angle
+            else -> 450.0 - angle // 180 < angle < 360
+        }
+    }
+
+    /**
+     * 将欧拉角格式化为字符串，同时显示弧度和度数
+     */
+    fun formatEulerAngles(eulerAngles: EulerAngles): String {
+        return eulerAngles.toList().joinToString(", ") { angle ->
+            String.format(Locale.CHINA, "%.4f rad (%.2f°)", angle, radiansToDegrees(angle))
+        }
+    }
+
+    /**
+     * 计算两点之间的俯仰角
+     *
+     * 范围为 ±180 度：
+     * - 0度：水平向前
+     * - +90度：垂直向上
+     * - -90度：垂直向下
+     * - ±180度：水平向后
+     */
+    fun calculatePitch(point1: Vector3, point2: Vector3): Double {
+        // 计算两点之间的差值
+        val dx = point2.x - point1.x
+        val dy = point2.y - point1.y
+        val dz = point2.z - point1.z
+
+        // 计算水平距离
+        val horizontalDistance = sqrt(dx * dx + dy * dy)
+
+        // 使用 atan2 计算俯仰角
+        var pitch = atan2(dz, horizontalDistance)
+
+        // 调整角度范围到 ±180 度
+        if (dx < 0) {
+            pitch = if (pitch > 0) {
+                PI - pitch
+            } else {
+                -PI - pitch
+            }
+        }
+
+        return pitch
+    }
+
+    /**
+     * 计算两点之间的偏航角
+     */
+    fun calculateYaw(point1: Vector3, point2: Vector3): Double {
+        // 计算水平平面上的差值
+        val dx = point2.x - point1.x
+        val dy = point2.y - point1.y
+
+        // 使用 atan2 计算偏航角
+        val yaw = atan2(dy, dx)
+
+        return yaw
+    }
+
+    /**
+     * 计算两点之间的横滚角
+     */
+    fun calculateRoll(point1: Vector3, point2: Vector3, upVector: Vector3): Double {
+        // 计算从 point1 到 point2 的方向向量并归一化
+        val direction = (point2 - point1).normalize()
+
+        // 计算右向量（叉乘得到垂直于 direction 和 up_vector 的向量）并归一化
+        val right = direction.cross(upVector).normalize()
+
+        // 计算新的上向量（叉乘得到垂直于 direction 和 right 的向量）
+        val newUp = right.cross(direction)
+
+        // 计算 roll 角（新的上向量与原始上向量之间的角度）
+        val cosRoll = newUp.dot(upVector)
+        var roll = acos(cosRoll.coerceIn(-1.0, 1.0))
+
+        // 确定 roll 的符号
+        if (right.dot(upVector) < 0) roll = -roll
+
+        return roll
+    }
+
+}

libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/ExcavatorPositionManager.kt

@@ -0,0 +1,306 @@
+package com.icegps.excavator.model
+
+import com.icegps.ahrs.AHRS401AParsedCanData
+import java.text.SimpleDateFormat
+import java.util.*
+
+/**
+ * @author xjh
+ * @date 2025/2/25
+ */
+class ExcavatorPositionManager(
+    private val excavatorPositionCalculator: ExcavatorPositionCalculator,
+    private var config: ExcavatorConfig,
+    private var isCalibrationMode: Boolean = false
+) {
+    companion object {
+        private const val TAG = "ExcavatorPositionManager"
+    }
+
+    // 回调接口定义
+    interface OnPositionCalculatedListener {
+        fun onPositionCalculated(logMessage: String)
+    }
+
+    // 回调列表
+    private val listeners = mutableListOf<OnPositionCalculatedListener>()
+
+    // 添加回调
+    fun addOnPositionCalculatedListener(listener: OnPositionCalculatedListener) {
+        if (!listeners.contains(listener)) {
+            listeners.add(listener)
+        }
+    }
+
+    // 移除回调
+    fun removeOnPositionCalculatedListener(listener: OnPositionCalculatedListener) {
+        listeners.remove(listener)
+    }
+
+    private var mainRTKData: RTKData? = null
+    private var subRTKData: RTKData? = null
+    private var attitudeData: RTKData? = null
+    var ahrsData: AHRS401AParsedCanData? = null
+    var subAhrsData: AHRS401AParsedCanData? = null
+    private var geoHelper: GeoHelper = GeoHelper.getSharedInstance()
+    private var keyPoints: ExcavatorKeyPoints? = null
+
+
+    /**
+     * 更新配置
+     */
+    fun updateConfig(newConfig: ExcavatorConfig) {
+        this.config = newConfig
+    }
+
+    /**
+     * 设置校准模式
+     */
+    fun setCalibrationMode(calibrationMode: Boolean) {
+        this.isCalibrationMode = calibrationMode
+        println("校准模式设置为: $calibrationMode")
+    }
+
+    /**
+     * 更新主天线RTK数据（位置）
+     */
+    private fun updateMainRTKData(data: RTKData) {
+        if (data.isMainAntenna()) {
+            mainRTKData = data
+            println("更新主RTK数据: $data")
+        }
+    }
+
+    /**
+     * 更新副天线RTK数据（位置）
+     */
+    private fun updateSubRTKData(data: RTKData) {
+        if (data.isSubAntenna()) {
+            subRTKData = data
+            println("更新副RTK数据: $data")
+        }
+    }
+
+    /**
+     * 更新RTK数据（自动判断是主天线、副天线还是姿态数据）
+     */
+    fun updateRtkData(data: RTKData) {
+        if (data.isMainAntenna()) {
+            println("主天线 RTK定位质量:${data.fixQuality} ")
+            updateMainRTKData(data)
+        } else if (data.isSubAntenna()) {
+            println("副天线 RTK定位质量:${data.fixQuality} ")
+            updateSubRTKData(data)
+        } else if (data.isAttitudeData()) {
+            updateAttitudeData(data)
+        }
+    }
+
+    /**
+     * 更新姿态数据
+     */
+    private fun updateAttitudeData(data: RTKData) {
+        if (data.isAttitudeData()) {
+            if (mainRTKData?.hasPositionData() == true && subRTKData?.hasPositionData() == true) {
+                attitudeData = data
+                println("更新姿态数据: $data")
+            }
+        }
+    }
+
+    /**
+     * 更新倾角传感器数据
+     */
+    fun updateAhrsData(data: AHRS401AParsedCanData) {
+        ahrsData = data
+//        println("更新AHRS传感器数据: Pitch=${data.pitch}, Roll=${data.roll}, Yaw=${data.yaw}")
+    }
+
+    fun updateSubAhrsData(data: AHRS401AParsedCanData) {
+        subAhrsData = data
+//        println("更新subAHRS传感器数据: Pitch=${data.pitch}, Roll=${data.roll}, Yaw=${data.yaw}")
+    }
+
+    fun calculateBucketPoints(): ExcavatorKeyPoints? {
+
+        if (mainRTKData == null || !mainRTKData!!.hasPositionData() || !mainRTKData!!.isRtkFixed()) {
+            println("主天线数据无效或未固定")
+            return null
+        }
+
+        if (subRTKData == null || !subRTKData!!.hasPositionData() || !subRTKData!!.isRtkFixed()) {
+            println("副天线数据无效或未固定")
+            return null
+        }
+
+        // 转换主RTK WGS84坐标到ENU坐标
+        val mainRtkEnu = geoHelper.wgs84ToENU(
+            mainRTKData!!.longitude,
+            mainRTKData!!.latitude,
+            mainRTKData!!.altitude
+        )
+        val mainRtkPos = Vector3(mainRtkEnu.x, mainRtkEnu.y, mainRtkEnu.z)
+
+        // 转换副RTK WGS84坐标到ENU坐标
+        val subRtkEnu = geoHelper.wgs84ToENU(
+            subRTKData!!.longitude,
+            subRTKData!!.latitude,
+            subRTKData!!.altitude
+        )
+        val subRtkPos = Vector3(subRtkEnu.x, subRtkEnu.y, subRtkEnu.z)
+
+        // 小臂传感器的欧拉角
+        val stickSensor = EulerAngles(
+            yaw = Math.toRadians(ahrsData!!.yaw.toDouble()),
+            pitch = Math.toRadians(ahrsData!!.pitch.toDouble()),
+            roll = Math.toRadians(ahrsData!!.roll.toDouble())
+        )
+
+        // 惰连杆传感器的欧拉角
+        val idleConnectingRodAngleSensor = EulerAngles(
+            yaw = Math.toRadians(subAhrsData!!.yaw.toDouble()),
+            pitch = Math.toRadians(subAhrsData!!.pitch.toDouble()),
+            roll = Math.toRadians(subAhrsData!!.roll.toDouble())
+        )
+
+        // 选择E点计算方法，校准模式下使用AE方法，校准后使用CE方法
+        val eCalculationMethod = if (isCalibrationMode) {
+            ECalculationMethod.USE_AE
+        } else {
+            ECalculationMethod.USE_CE
+        }
+
+        println(
+            "传参:\n" +
+                    "mainRtkPos: $mainRtkPos\n" +
+                    "subRtkPos: $subRtkPos\n" +
+                    "rtkConfig: ${config.rtkConfig}\n" +
+                    "excavatorDims: ${config.excavatorDims}\n" +
+                    "linkageLengths: ${config.linkageLengths}\n" +
+                    "stickAngleSensor: $stickSensor\n" +
+                    "idleConnectingRodAngleSensor: $idleConnectingRodAngleSensor\n" +
+                    "antennaInstallationMode: ${config.antennaInstallationMode}\n" +
+                    "sensorInstallationMode: ${config.sensorInstallationMode}\n" +
+                    "eCalculationMethod: $eCalculationMethod\n" +
+                    "isCalibrationMode: $isCalibrationMode"
+        )
+
+        // 计算铲斗位置
+        keyPoints = excavatorPositionCalculator.calculateExcavatorKeyPoints(
+            mainRtkPos = mainRtkPos,
+            subRtkPos = subRtkPos,
+            rtkConfig = config.rtkConfig,
+            excavatorDims = config.excavatorDims,
+            linkageLengths = config.linkageLengths,
+            stickAngleSensor = stickSensor,
+            idleConnectingRodAngleSensor = idleConnectingRodAngleSensor,
+            antennaInstallationMode = config.antennaInstallationMode,
+            sensorInstallationMode = config.sensorInstallationMode,
+            stickOffsetEulerAngles = config.stickOffsetEulerAngles,
+            eCalculationMethod = eCalculationMethod,
+            idleConnectingRodOffsetEulerAngles = config.idleConnectingRodOffsetEulerAngles
+        )
+        notifyListeners(true, keyPoints, stickSensor, idleConnectingRodAngleSensor)
+//        println("铲斗位置计算成功——经纬度: ${getKeyPointsWGS84Positions()}")
+        return keyPoints
+    }
+
+    fun getKeyPointsWGS84Positions(): Map<String, Triple<Double, Double, Double>>? {
+        return keyPoints?.let { keyPoints ->
+            mapOf(
+                "joint1" to convertToWGS84(keyPoints.joint1),
+                "joint2" to convertToWGS84(keyPoints.joint2),
+                "joint3" to convertToWGS84(keyPoints.joint3),
+                "A" to convertToWGS84(keyPoints.A),
+                "B" to convertToWGS84(keyPoints.B),
+                "C" to convertToWGS84(keyPoints.C),
+                "D" to convertToWGS84(keyPoints.D),
+                "E" to convertToWGS84(keyPoints.E),
+                "E1" to convertToWGS84(keyPoints.E1),
+                "E2" to convertToWGS84(keyPoints.E2)
+            )
+        }
+    }
+
+    private fun convertToWGS84(vector: Vector3): Triple<Double, Double, Double> {
+        val enu = GeoHelper.Enu(vector.x, vector.y, vector.z)
+        val wgs84 = geoHelper.enuToWGS84(enu)
+        return Triple(wgs84[0], wgs84[1], wgs84[2]) // 经度, 纬度, 高度
+    }
+
+    private fun notifyListeners(
+        success: Boolean,
+        keyPoints: ExcavatorKeyPoints?,
+        stickSensor: EulerAngles?,
+        idleConnectingRodAngleSensor: EulerAngles?
+    ) {
+        val timestamp = Date()
+        val dateFormat = SimpleDateFormat("HH:mm:ss.SSS", Locale.CHINA)
+        val timeString = dateFormat.format(timestamp)
+
+        // 构建日志消息
+        val logMessage = buildString {
+            append("===== $timeString =====\n")
+
+            // 添加RTK数据信息
+            append("RTK数据状态:\n")
+            mainRTKData?.let {
+                append("主天线: 经度=${String.format("%.8f", it.longitude)}, ")
+                append("纬度=${String.format("%.8f", it.latitude)}, ")
+                append("高度=${String.format("%.3f", it.altitude)}m, ")
+                append("质量=${it.fixQuality}\n")
+            } ?: append("主天线: 无数据\n")
+
+            subRTKData?.let {
+                append("副天线: 经度=${String.format("%.8f", it.longitude)}, ")
+                append("纬度=${String.format("%.8f", it.latitude)}, ")
+                append("高度=${String.format("%.3f", it.altitude)}m, ")
+                append("质量=${it.fixQuality}\n")
+            } ?: append("副天线: 无数据\n")
+
+            // 添加传感器数据
+            stickSensor?.let {
+                append("传感器0: Roll=${String.format("%.2f", Math.toDegrees(it.roll))}°, ")
+                append("Pitch=${String.format("%.2f", Math.toDegrees(it.pitch))}°, ")
+                append("Yaw=${String.format("%.2f", Math.toDegrees(it.yaw))}°\n")
+            } ?: append("传感器0: 无数据\n")
+
+            idleConnectingRodAngleSensor?.let {
+                append("传感器1: Roll=${String.format("%.2f", Math.toDegrees(it.roll))}°, ")
+                append("Pitch=${String.format("%.2f", Math.toDegrees(it.pitch))}°, ")
+                append("Yaw=${String.format("%.2f", Math.toDegrees(it.yaw))}°\n")
+            } ?: append("传感器1: 无数据\n")
+
+            // 添加计算方法和CE长度信息
+            val eCalculationMethod = if (isCalibrationMode) {
+                ECalculationMethod.USE_AE
+            } else {
+                ECalculationMethod.USE_CE
+            }
+            append("【计算方法】: $eCalculationMethod\n")
+            append("【连杆长度】: ${config.linkageLengths}\n\n")
+
+            // 添加计算结果
+            if (success && keyPoints != null) {
+                append("铲斗位置计算成功:\n")
+                val wgs84Positions = getKeyPointsWGS84Positions()
+                wgs84Positions?.forEach { (name, position) ->
+                    val (lon, lat, alt) = position
+                    append("$name: 经度=${String.format("%.8f", lon)}, ")
+                    append("纬度=${String.format("%.8f", lat)}, ")
+                    append("高度=${String.format("%.3f", alt)}m\n")
+                }
+            } else {
+                append("铲斗位置计算失败\n")
+            }
+
+            append("\n")
+        }
+
+        // 通知所有监听器
+        val listenersCopy = ArrayList(listeners) // 创建副本避免并发修改
+        listenersCopy.forEach { listener ->
+            listener.onPositionCalculated(logMessage)
+        }
+    }
+}

libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/GeoHelper.kt

@@ -0,0 +1,243 @@
+package com.icegps.excavator.model
+
+import kotlin.math.*
+
+/**
+ * WGS84、EPSG3857、ENU 的坐标转换工具类
+ *
+ * @author lm
+ * @date 2024/8/2
+ */
+class GeoHelper private constructor() {
+
+    companion object {
+        private var sharedInstance: GeoHelper? = null
+
+        fun getSharedInstance(): GeoHelper {
+            return sharedInstance ?: GeoHelper().also { sharedInstance = it }
+        }
+
+        fun createInstance(): GeoHelper = GeoHelper()
+    }
+
+    // WGS-84 ellipsoid parameters
+    private val RADIUS = 6378137.0 // Major radius
+    private val RADIUS_B = 6356752.314245 // Minor radius
+    private val E = (RADIUS * RADIUS - RADIUS_B * RADIUS_B) / (RADIUS * RADIUS) // Eccentricity
+    private val HALF_SIZE = Math.PI * RADIUS // Half circumference of Earth
+    private val DEG2RAD = Math.PI / 180 // Degrees to radians conversion factor
+    private val RAD2DEG = 180 / Math.PI // Radians to degrees conversion factor
+    private val RE_WGS84 = 6378137.0 // Earth's equatorial radius in WGS84
+    private val FE_WGS84 = 1.0 / 298.257223563 // Flattening of the WGS84 ellipsoid
+
+    private var isFirstPoint = true
+    private val bPos = DoubleArray(3)
+    private var bECEF = DoubleArray(3)
+    private val rPos = DoubleArray(3)
+    private var rECEF = DoubleArray(3)
+    private val vECEF = DoubleArray(3)
+
+    private var useBlhToEnu: Boolean = true
+    private var blhToEnu: BlhToEnu = BlhToEnu()
+
+    /**
+     * 重置 ENU 基准点
+     * 调用此方法后，下一次 wgs84ToENU 调用将设置新的基准点
+     */
+    fun resetEnuBenchmarkPoint() {
+        if (useBlhToEnu) {
+            blhToEnu.resetEnuBenchmarkPoint()
+            return
+        }
+
+        isFirstPoint = true
+    }
+
+    /**
+     * 将 WGS84 坐标转换为 ENU (East-North-Up) 坐标
+     * 如果是第一个点，它将被设置为 ENU 坐标系的基准点
+     *
+     * @param lon 经度（度）
+     * @param lat 纬度（度）
+     * @param alt 高度（米）
+     * @return 包含 ENU 坐标的 Enu 对象
+     */
+    fun wgs84ToENU(lon: Double, lat: Double, alt: Double): Enu {
+        if (useBlhToEnu) {
+            val enu = blhToEnu.wgs84ToEnu(lon = lon, lat = lat, height = alt)
+            return Enu(enu[0], enu[1], enu[2])
+        }
+
+        if (isFirstPoint) setEnuBenchmark(lon, lat, alt)
+        rPos[0] = lat * DEG2RAD
+        rPos[1] = lon * DEG2RAD
+        rPos[2] = alt
+
+        rECEF = pos2ecef(rPos)
+        vECEF[0] = rECEF[0] - bECEF[0]
+        vECEF[1] = rECEF[1] - bECEF[1]
+        vECEF[2] = rECEF[2] - bECEF[2]
+        val enuDoubleArray = ecef2enu(bPos, vECEF)
+        return Enu(enuDoubleArray[0], enuDoubleArray[1], enuDoubleArray[2])
+    }
+
+    /**
+     * 设置 ENU 坐标系的基准点
+     *
+     * @param lon 基准点经度（度）
+     * @param lat 基准点纬度（度）
+     * @param alt 基准点高度（米）
+     */
+    private fun setEnuBenchmark(lon: Double, lat: Double, alt: Double) {
+        bPos[0] = lat * DEG2RAD
+        bPos[1] = lon * DEG2RAD
+        bPos[2] = alt
+        bECEF = pos2ecef(bPos)
+        isFirstPoint = false
+    }
+
+    /**
+     * 将 ENU 坐标转换为 WGS84 坐标
+     *
+     * @param enu 包含 ENU 坐标的 Enu 对象
+     * @return 包含 WGS84 坐标 {经度, 纬度, 高度} 的 DoubleArray
+     */
+    fun enuToWGS84(enu: Enu): DoubleArray {
+        if (useBlhToEnu) {
+            val wgs84 = blhToEnu.enuToWgs84(e = enu.x, n = enu.y, u = enu.z)
+            return doubleArrayOf(wgs84[1], wgs84[0], wgs84[2])
+        }
+
+        val enuArray = doubleArrayOf(enu.x, enu.y, enu.z)
+        val enuToEcefMatrix = xyz2enu(bPos)
+        val ecefArray = matmul(charArrayOf('T', 'N'), 3, 1, 3, 1.0, enuToEcefMatrix, enuArray, 0.0)
+        vECEF[0] = bECEF[0] + ecefArray[0]
+        vECEF[1] = bECEF[1] + ecefArray[1]
+        vECEF[2] = bECEF[2] + ecefArray[2]
+        return ecef2pos(vECEF)
+    }
+
+    /**
+     * 将 WGS84 坐标转换为 EPSG3857 坐标
+     *
+     * @param lon 经度（度）
+     * @param lat 纬度（度）
+     * @return 包含 EPSG3857 坐标的 EPSG3857 对象
+     */
+    fun wgs84ToEPSG3857(lon: Double, lat: Double): EPSG3857 {
+        val x = lon * HALF_SIZE / 180
+        var y = RADIUS * ln(tan(Math.PI * (lat + 90) / 360))
+        y = y.coerceIn(-HALF_SIZE, HALF_SIZE)
+        return EPSG3857(x, y)
+    }
+
+    /**
+     * 将 EPSG3857 坐标转换为 WGS84 坐标
+     *
+     * @param epsg3857 包含 EPSG3857 坐标的 EPSG3857 对象
+     * @return 包含 WGS84 坐标 {经度, 纬度} 的 DoubleArray
+     */
+    fun epsg3857ToWGS84(epsg3857: EPSG3857): DoubleArray {
+        val lon = (epsg3857.x / HALF_SIZE) * 180.0
+        val lat = (2 * atan(exp(epsg3857.y / RADIUS)) - Math.PI / 2) * RAD2DEG
+        return doubleArrayOf(lon, lat)
+    }
+
+    private fun pos2ecef(pos: DoubleArray): DoubleArray {
+        val (lat, lon, alt) = pos
+        val sinp = sin(lat)
+        val cosp = cos(lat)
+        val sin_l = sin(lon)
+        val cos_l = cos(lon)
+        val e2 = FE_WGS84 * (2.0 - FE_WGS84)
+        val v = RE_WGS84 / sqrt(1.0 - e2 * sinp * sinp)
+
+        return doubleArrayOf(
+            (v + alt) * cosp * cos_l,
+            (v + alt) * cosp * sin_l,
+            (v * (1.0 - e2) + alt) * sinp
+        )
+    }
+
+    private fun ecef2enu(pos: DoubleArray, r: DoubleArray): DoubleArray {
+        val E = xyz2enu(pos)
+        return matmul(charArrayOf('N', 'N'), 3, 1, 3, 1.0, E, r, 0.0)
+    }
+
+    private fun matmul(
+        tr: CharArray,
+        n: Int,
+        k: Int,
+        m: Int,
+        alpha: Double,
+        A: DoubleArray,
+        B: DoubleArray,
+        beta: Double
+    ): DoubleArray {
+        val f = when {
+            tr[0] == 'N' && tr[1] == 'N' -> 1
+            tr[0] == 'N' && tr[1] == 'T' -> 2
+            tr[0] == 'T' && tr[1] == 'N' -> 3
+            else -> 4
+        }
+        val C = DoubleArray(n * k)
+        for (i in 0 until n) {
+            for (j in 0 until k) {
+                var d = 0.0
+                when (f) {
+                    1 -> for (x in 0 until m) d += A[i + x * n] * B[x + j * m]
+                    2 -> for (x in 0 until m) d += A[i + x * n] * B[j + x * k]
+                    3 -> for (x in 0 until m) d += A[x + i * m] * B[x + j * m]
+                    4 -> for (x in 0 until m) d += A[x + i * m] * B[j + x * k]
+                }
+                C[i + j * n] = alpha * d + beta * C[i + j * n]
+            }
+        }
+        return C
+    }
+
+    private fun xyz2enu(pos: DoubleArray): DoubleArray {
+        val (lat, lon) = pos
+        val sinp = sin(lat)
+        val cosp = cos(lat)
+        val sin_l = sin(lon)
+        val cos_l = cos(lon)
+
+        return doubleArrayOf(
+            -sin_l, cos_l, 0.0,
+            -sinp * cos_l, -sinp * sin_l, cosp,
+            cosp * cos_l, cosp * sin_l, sinp
+        )
+    }
+
+    private fun ecef2pos(ecef: DoubleArray): DoubleArray {
+        val (x, y, z) = ecef
+        val a = RE_WGS84
+        val b = a * (1 - FE_WGS84)
+        val e2 = (a * a - b * b) / (a * a)
+        val e2p = (a * a - b * b) / (b * b)
+        val r2 = x * x + y * y
+        val r = sqrt(r2)
+        val E2 = a * a - b * b
+        val F = 54 * b * b * z * z
+        val G = r2 + (1 - e2) * z * z - e2 * E2
+        val c = (e2 * e2 * F * r2) / (G * G * G)
+        val s = cbrt(1 + c + sqrt(c * c + 2 * c))
+        val P = F / (3 * (s + 1 / s + 1) * (s + 1 / s + 1) * G * G)
+        val Q = sqrt(1 + 2 * e2 * e2 * P)
+        val r0 =
+            -(P * e2 * r) / (1 + Q) + sqrt(0.5 * a * a * (1 + 1.0 / Q) - P * (1 - e2) * z * z / (Q * (1 + Q)) - 0.5 * P * r2)
+        val U = sqrt((r - e2 * r0) * (r - e2 * r0) + z * z)
+        val V = sqrt((r - e2 * r0) * (r - e2 * r0) + (1 - e2) * z * z)
+        val Z0 = b * b * z / (a * V)
+
+        val lon = atan2(y, x) * RAD2DEG
+        val lat = atan((z + e2p * Z0) / r) * RAD2DEG
+        val alt = U * (1 - b * b / (a * V))
+        return doubleArrayOf(lon, lat, alt)
+    }
+
+    data class EPSG3857(var x: Double = 0.0, var y: Double = 0.0)
+
+    data class Enu(var x: Double = 0.0, var y: Double = 0.0, var z: Double = 0.0)
+}

libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/RTKData.kt

@@ -0,0 +1,56 @@
+package com.icegps.excavator.model
+
+/**
+ * @author xjh
+ * @date 2025/2/25
+ */
+
+/**
+ * 数据类，存储RTK位置信息和姿态信息（包括航向角、俯仰角、滚转角）
+ */
+data class RTKData(
+    val latitude: Double = 0.0,         // 纬度，单位：度
+    val longitude: Double = 0.0,        // 经度，单位：度
+    val altitude: Double = 0.0,         // 高度，单位：米
+    val fixQuality: Int = 0,            // 定位质量：0=未定，1=单点，2=伪距，4=固定，5=浮动
+    val satellites: Int = 0,            // 使用的卫星数量
+    val hdop: Double = 0.0,             // 水平精度因子
+    val time: String = "",              // 时间戳（用于所有类型）
+    val messageType: String = "GNGGA",  // 消息类型：GNGGA、GNGGAH或GNHPR
+    val yaw: Double = 0.0,              // 航向角，单位：度
+    val pitch: Double = 0.0,            // 俯仰角，单位：度
+    val roll: Double = 0.0              // 滚转角，单位：度
+) {
+    /**
+     * 检查RTK是否固定
+     */
+    fun isRtkFixed(): Boolean = fixQuality == 4
+
+    /**
+     * 检查是否为主天线数据
+     */
+    fun isMainAntenna(): Boolean = messageType == "GNGGA"
+
+    /**
+     * 检查是否为副天线数据
+     */
+    fun isSubAntenna(): Boolean = messageType == "GNGGAH"
+
+    /**
+     * 检查是否为姿态数据（如航向、俯仰、滚转）
+     */
+    fun isAttitudeData(): Boolean = messageType == "GNHPR"
+
+    /**
+     * 是否为有效的位置数据（主或副天线）
+     */
+    fun hasPositionData(): Boolean = latitude != 0.0 && longitude != 0.0 && altitude != 0.0
+
+    override fun toString(): String {
+        return if (isAttitudeData()) {
+            "RTKData($messageType: heading=$yaw, pitch=$pitch, roll=$roll, time=$time)"
+        } else {
+            "RTKData($messageType: lat=$latitude, lon=$longitude, alt=$altitude, fix=$fixQuality, sats=$satellites, hdop=$hdop, time=$time)"
+        }
+    }
+}