From 784f2324bdee004350772e502bed875cffe51ee3 Mon Sep 17 00:00:00 2001 From: tabidachinokaze Date: Fri, 30 May 2025 00:41:39 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E6=B7=BB=E5=8A=A0=20ahrs=20=E5=92=8C?= =?UTF-8?q?=20excavator-bucket-position=20=E6=A8=A1=E5=9D=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- build.gradle.kts | 3 + libs/ahrs/build.gradle.kts | 7 + .../ahrs/AHRS401ACanConfigurationData.kt | 159 +++++ .../com/icegps/ahrs/AHRS401ACanDataParser.kt | 191 ++++++ .../com/icegps/ahrs/AHRS401ACanFrame.kt | 22 + .../com/icegps/ahrs/AHRS401ACanManager.kt | 234 +++++++ .../com/icegps/ahrs/AHRS401AParsedCanData.kt | 30 + .../main/kotlin/com/icegps/ahrs/ShellUtils.kt | 93 +++ .../com/icegps/ahrs/android_socketcan.java | 16 + .../build.gradle.kts | 11 + .../com/icegps/excavator/model/BlhToEnu.kt | 263 ++++++++ .../icegps/excavator/model/ExcavatorConfig.kt | 62 ++ .../model/ExcavatorPositionCalculator.kt | 603 ++++++++++++++++++ .../model/ExcavatorPositionManager.kt | 306 +++++++++ .../com/icegps/excavator/model/GeoHelper.kt | 243 +++++++ .../com/icegps/excavator/model/RTKData.kt | 56 ++ settings.gradle.kts | 5 +- 17 files changed, 2303 insertions(+), 1 deletion(-) create mode 100644 libs/ahrs/build.gradle.kts create mode 100644 libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401ACanConfigurationData.kt create mode 100644 libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401ACanDataParser.kt create mode 100644 libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401ACanFrame.kt create mode 100644 libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401ACanManager.kt create mode 100644 libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401AParsedCanData.kt create mode 100644 libs/ahrs/src/main/kotlin/com/icegps/ahrs/ShellUtils.kt create mode 100644 libs/ahrs/src/main/kotlin/com/icegps/ahrs/android_socketcan.java create mode 100644 libs/excavator-bucket-position/build.gradle.kts create mode 100644 libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/BlhToEnu.kt create mode 100644 libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/ExcavatorConfig.kt create mode 100644 libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/ExcavatorPositionCalculator.kt create mode 100644 libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/ExcavatorPositionManager.kt create mode 100644 libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/GeoHelper.kt create mode 100644 libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/RTKData.kt diff --git a/build.gradle.kts b/build.gradle.kts index 7e041e6..659a6d2 100644 --- a/build.gradle.kts +++ b/build.gradle.kts @@ -13,4 +13,7 @@ kotlin { dependencies { add("commonMainApi", project(":deps")) + + jvmMainImplementation(project(":libs:excavator-bucket-position")) + jvmMainImplementation(project(":libs:ahrs")) } \ No newline at end of file diff --git a/libs/ahrs/build.gradle.kts b/libs/ahrs/build.gradle.kts new file mode 100644 index 0000000..dc719b4 --- /dev/null +++ b/libs/ahrs/build.gradle.kts @@ -0,0 +1,7 @@ +plugins { + kotlin("jvm") +} + +repositories { + mavenCentral() +} \ No newline at end of file diff --git a/libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401ACanConfigurationData.kt b/libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401ACanConfigurationData.kt new file mode 100644 index 0000000..04fd349 --- /dev/null +++ b/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) + } + } +} \ No newline at end of file diff --git a/libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401ACanDataParser.kt b/libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401ACanDataParser.kt new file mode 100644 index 0000000..f6b1ad7 --- /dev/null +++ b/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() + 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() + } +} \ No newline at end of file diff --git a/libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401ACanFrame.kt b/libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401ACanFrame.kt new file mode 100644 index 0000000..5a9e025 --- /dev/null +++ b/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" } + } + +} \ No newline at end of file diff --git a/libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401ACanManager.kt b/libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401ACanManager.kt new file mode 100644 index 0000000..684b737 --- /dev/null +++ b/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() + 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 + } +} \ No newline at end of file diff --git a/libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401AParsedCanData.kt b/libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401AParsedCanData.kt new file mode 100644 index 0000000..ab10dd7 --- /dev/null +++ b/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() + } +} \ No newline at end of file diff --git a/libs/ahrs/src/main/kotlin/com/icegps/ahrs/ShellUtils.kt b/libs/ahrs/src/main/kotlin/com/icegps/ahrs/ShellUtils.kt new file mode 100644 index 0000000..2741b6b --- /dev/null +++ b/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?, isRoot: Boolean = false, isNeedResultMsg: Boolean = true): CommandResult { + return execCommand(commands?.toTypedArray(), isRoot, isNeedResultMsg) + } + + fun execCommand(commands: Array?, 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? + ) + +} \ No newline at end of file diff --git a/libs/ahrs/src/main/kotlin/com/icegps/ahrs/android_socketcan.java b/libs/ahrs/src/main/kotlin/com/icegps/ahrs/android_socketcan.java new file mode 100644 index 0000000..1414b97 --- /dev/null +++ b/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); +} \ No newline at end of file diff --git a/libs/excavator-bucket-position/build.gradle.kts b/libs/excavator-bucket-position/build.gradle.kts new file mode 100644 index 0000000..64acfeb --- /dev/null +++ b/libs/excavator-bucket-position/build.gradle.kts @@ -0,0 +1,11 @@ +plugins { + kotlin("jvm") +} + +repositories { + mavenCentral() +} + +dependencies { + implementation(project(":libs:ahrs")) +} \ No newline at end of file diff --git a/libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/BlhToEnu.kt b/libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/BlhToEnu.kt new file mode 100644 index 0000000..56dc065 --- /dev/null +++ b/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 { + 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 { + 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 { + 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, matB: Array): Array { + 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, pt: DoubleArray): DoubleArray { + return DoubleArray(3) { j -> + (0..2).sumOf { i -> mat[j][i] * pt[i] } + } + } + +} \ No newline at end of file diff --git a/libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/ExcavatorConfig.kt b/libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/ExcavatorConfig.kt new file mode 100644 index 0000000..4accb8d --- /dev/null +++ b/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) +) \ No newline at end of file diff --git a/libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/ExcavatorPositionCalculator.kt b/libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/ExcavatorPositionCalculator.kt new file mode 100644 index 0000000..78f6a8f --- /dev/null +++ b/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 = 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 { + // 计算两个球心之间的向量 + 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 + } + +} \ No newline at end of file diff --git a/libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/ExcavatorPositionManager.kt b/libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/ExcavatorPositionManager.kt new file mode 100644 index 0000000..8a8c5e9 --- /dev/null +++ b/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() + + // 添加回调 + 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>? { + 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 { + 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) + } + } +} \ No newline at end of file diff --git a/libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/GeoHelper.kt b/libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/GeoHelper.kt new file mode 100644 index 0000000..bcbfdf7 --- /dev/null +++ b/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) +} \ No newline at end of file diff --git a/libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/RTKData.kt b/libs/excavator-bucket-position/src/main/kotlin/com/icegps/excavator/model/RTKData.kt new file mode 100644 index 0000000..fe36aeb --- /dev/null +++ b/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)" + } + } +} \ No newline at end of file diff --git a/settings.gradle.kts b/settings.gradle.kts index 7dc0862..f643433 100644 --- a/settings.gradle.kts +++ b/settings.gradle.kts @@ -22,4 +22,7 @@ buildscript { apply(plugin = "com.soywiz.korge.settings") -rootProject.name = "korge" \ No newline at end of file +rootProject.name = "korge" + +include(":libs:excavator-bucket-position") +include(":libs:ahrs") \ No newline at end of file