feat: 添加 ahrs 和 excavator-bucket-position 模块
This commit is contained in:
7
libs/ahrs/build.gradle.kts
Normal file
7
libs/ahrs/build.gradle.kts
Normal file
@@ -0,0 +1,7 @@
|
||||
plugins {
|
||||
kotlin("jvm")
|
||||
}
|
||||
|
||||
repositories {
|
||||
mavenCentral()
|
||||
}
|
||||
@@ -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)
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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()
|
||||
}
|
||||
}
|
||||
@@ -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" }
|
||||
}
|
||||
|
||||
}
|
||||
234
libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401ACanManager.kt
Normal file
234
libs/ahrs/src/main/kotlin/com/icegps/ahrs/AHRS401ACanManager.kt
Normal file
@@ -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
|
||||
}
|
||||
}
|
||||
@@ -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()
|
||||
}
|
||||
}
|
||||
93
libs/ahrs/src/main/kotlin/com/icegps/ahrs/ShellUtils.kt
Normal file
93
libs/ahrs/src/main/kotlin/com/icegps/ahrs/ShellUtils.kt
Normal file
@@ -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?
|
||||
)
|
||||
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
Reference in New Issue
Block a user