移植bot4cj
This commit is contained in:
qing committed
1 parent
7803843155
commit
78ad3e6d57
76 files changed
+14468
No files matched your search
@@ -0,0 +1,339 @@
|
||||
|
||||
/* TODO(20260518-qing): 稍后我们专门做一个i2c代码仓库，这样就能够区分不同架构上面的int的大小
|
||||
* TODO(20260615-qing): 有没有可能只做一个简单的封装不引入异常机制 */
|
||||
|
||||
package bot4cj.hd
|
||||
import pthread4cj.*
|
||||
import ctype4cj.*
|
||||
|
||||
/* -------- 我们将直接调用glibc中的一些函数 -------- */
|
||||
foreign func ioctl(fd: C_Int, addr: C_ULong, ...): C_Int /* -------- 成功返回非负整数，失败返回-1 -------- */
|
||||
|
||||
const I2C_FAILED = -1i32 /* -------- I2C关闭文件失败 -------- */
|
||||
const I2C_SUCCESS = 0i32 /* -------- I2C关闭文件成功 -------- */
|
||||
const I2C_SLAVE = 0x0703u64 /* -------- at least on arm64 -------- */
|
||||
|
||||
/* -------- OPEN I2C DEVICE 打开I2C设备 -------- */
|
||||
@C
|
||||
public func openI2cDev(path: CString): Int32 {
|
||||
unsafe {
|
||||
let fd = libc_open(path, O_RDWR)
|
||||
if (fd > 0) {
|
||||
I2cLock_create(fd) /* -------- 为I2C设备创建锁 -------- */
|
||||
return fd
|
||||
}
|
||||
else {
|
||||
throw I2cOpenedFailed("?")
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ---- 简化版，判断在外部实现 ---- */
|
||||
@C
|
||||
public func openI2cDev1(path: CString): C_Int {
|
||||
unsafe {
|
||||
let fd = libc_open(path, O_RDWR)
|
||||
I2cLock_create(fd)
|
||||
return fd
|
||||
}
|
||||
}
|
||||
|
||||
/* -------- CLOSE I2C DEVICE 关闭I2C设备 -------- */
|
||||
@C
|
||||
public func closeI2cDev(fd: Int32): Unit {
|
||||
unsafe {
|
||||
I2cLock_destroy(fd) /* -------- 销毁I2C设备的锁 -------- */
|
||||
let ret = libc_close(fd)
|
||||
if (ret == I2C_FAILED) {
|
||||
throw I2cClosedFailed("?") }
|
||||
else if (ret == I2C_SUCCESS) { ; }
|
||||
else {
|
||||
throw I2cUnknowExp(
|
||||
"?", "$function returned: {ret.toString()}")
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@C
|
||||
public func closeI2cDev1(fd: C_Int): Unit {
|
||||
unsafe {
|
||||
I2cLock_destroy(fd)
|
||||
libc_close(fd)
|
||||
}
|
||||
}
|
||||
|
||||
/* -------- SET I2C ADDR 设置I2C地址 -------- */
|
||||
@C
|
||||
public func setI2cAddr(fd: Int32, addr: UInt8): Unit {
|
||||
unsafe {
|
||||
let ret = ioctl(fd, I2C_SLAVE, addr)
|
||||
if (ret < 0) {
|
||||
throw I2cSetAddressFailed("?") }
|
||||
}
|
||||
}
|
||||
|
||||
@C
|
||||
public func setI2cAddr1(fd: C_Int, addr: UInt8): C_Int {
|
||||
unsafe {
|
||||
return ioctl(fd, I2C_SLAVE, addr)
|
||||
}
|
||||
}
|
||||
|
||||
/* -------- WRITE I2C REG 写I2C寄存器 -------- */
|
||||
@C
|
||||
public func writeI2cReg(fd: Int32, reg: UInt8, val:UInt8): Unit {
|
||||
var a: VArray<UInt8, $2> = [reg, val]
|
||||
var p: CPointer<Unit> = unsafe { CPointer<Unit>(inout a) }
|
||||
if (2 != unsafe { libc_write(fd, p, 2) }) {
|
||||
throw I2cWriteRegFailed("?")
|
||||
}
|
||||
}
|
||||
|
||||
/* -------- READ I2C REG 读I2C寄存器 --------
|
||||
* INFO(20260522-qing): 读取寄存器之前要先写寄存器地址
|
||||
* INFO(20260616-qing): 这个函数只读取一个字节的数据
|
||||
* -------- */
|
||||
@C
|
||||
public func readI2cReg(fd: Int32, reg: UInt8): Int8 {
|
||||
var ptr = reg
|
||||
if (1 != unsafe { libc_write(fd, CPointer<Unit>(inout ptr), 1) }) {
|
||||
throw I2cReadRegFailed("?")
|
||||
}
|
||||
var value: Int8 = 0
|
||||
var p = unsafe { CPointer<Unit>(inout value) }
|
||||
let ret = unsafe { libc_read(fd, p, 1) }
|
||||
if (ret != 1) { throw I2cReadRegFailed("?") }
|
||||
return value
|
||||
}
|
||||
|
||||
|
||||
/* -------- I2C锁具 --------
|
||||
* INFO(20260521-qing): 因为每个I2C接口在通信时具有原子化的需求
|
||||
* - 需要一个互斥锁集合，在打开第一个I2C设备时被初始化，
|
||||
* - 释放最后一个I2C设备时，将锁具释放
|
||||
* - **每次进行I2C通信时，进行上锁操作** */
|
||||
@C
|
||||
struct I2C_Lock {
|
||||
var i2c_fd: Int32 = 0 /* -------- 这个锁绑定的I2C设备文件句柄 -------- */
|
||||
var lock = CPointer<UInt8>() /* -------- phread_mutex_t 锁对象 -------- */
|
||||
var is_active = false /* -------- 该锁具是否被激活 --------*/
|
||||
}
|
||||
|
||||
|
||||
var MAIN_LOCK = CPointer<UInt8>() /* -------- 主锁，操作锁具时使用 -------- */
|
||||
var LOCKS = CPointer<I2C_Lock>() /* -------- 指向锁具内存的指针 -------- */
|
||||
const LOCKS_SIZE: Int64 = 20 /* -------- 锁具的容量 --------*/
|
||||
|
||||
/* -------- 为一个I2C设备注册锁具 -------- */
|
||||
@C
|
||||
func I2cLock_create(fd: Int32): Unit {
|
||||
unsafe {
|
||||
/* -------- 初始化主要的锁 -------- */
|
||||
if (MAIN_LOCK.isNull()) {
|
||||
MAIN_LOCK = _mutex_create()
|
||||
LOCKS = LibC.malloc<I2C_Lock>(count: LOCKS_SIZE)
|
||||
for (i in 0..20) {
|
||||
let tmp = I2C_Lock()
|
||||
(LOCKS+i).write(tmp)
|
||||
}
|
||||
}
|
||||
|
||||
/* -------- 获取主锁 --------*/
|
||||
_mutex_lock(MAIN_LOCK)
|
||||
var flag = false
|
||||
|
||||
for (idx in 0..LOCKS_SIZE) {
|
||||
var lock = (LOCKS+idx).read()
|
||||
if (lock.is_active == false) {
|
||||
lock.is_active = true
|
||||
lock.i2c_fd = fd
|
||||
lock.lock = _mutex_create()
|
||||
_mutex_init(lock.lock)
|
||||
(LOCKS+idx).write(lock)
|
||||
flag = true
|
||||
break
|
||||
}
|
||||
}
|
||||
|
||||
_mutex_unlock(MAIN_LOCK)
|
||||
if (flag ) { return } /* -------- 出口 -------- */
|
||||
throw OverflowException("LOCKS is over.")
|
||||
}
|
||||
}
|
||||
/* -------- 为一个I2C设备注销锁具 -------- */
|
||||
@C
|
||||
func I2cLock_destroy(fd: Int32): Unit {
|
||||
unsafe {
|
||||
/* -------- 检查锁具有没有被初始化 --------*/
|
||||
if (MAIN_LOCK.isNull()) {
|
||||
throw NoneValueException("no i2c lock")
|
||||
}
|
||||
_mutex_lock(MAIN_LOCK)
|
||||
|
||||
var flag_any = false
|
||||
var flag_target = false
|
||||
for (idx in 0..LOCKS_SIZE) {
|
||||
var lock = (LOCKS+idx).read()
|
||||
if (lock.is_active == true && lock.i2c_fd == fd) {
|
||||
lock.is_active = false
|
||||
_mutex_destroy(lock.lock)
|
||||
_mutex_free(lock.lock)
|
||||
(LOCKS+idx).write(lock)
|
||||
flag_target = true
|
||||
/* -------- NOTE: 扫描整个锁具不终止 -------- */
|
||||
}
|
||||
else if (lock.is_active == true && lock.i2c_fd != fd) {
|
||||
flag_any = true
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
_mutex_unlock(MAIN_LOCK)
|
||||
|
||||
/* -------- 如果锁具里已经没有I2C锁， 就释放整个锁具 -------- */
|
||||
if (!flag_any) {
|
||||
LibC.free(LOCKS)
|
||||
_mutex_free(MAIN_LOCK)
|
||||
MAIN_LOCK = CPointer<UInt8>()
|
||||
}
|
||||
|
||||
/* -------- 出口 -------- */
|
||||
if (flag_target) {
|
||||
return }
|
||||
|
||||
|
||||
throw NoneValueException("i2c lock nor found")
|
||||
}
|
||||
}
|
||||
|
||||
/* -------- 为一个I2C锁具加锁 -------- */
|
||||
@C
|
||||
func I2cLock_lock(fd: Int32): Unit {
|
||||
unsafe {
|
||||
if (MAIN_LOCK.isNull()) {
|
||||
throw NoneValueException("no i2c lock")
|
||||
}
|
||||
|
||||
_mutex_lock(MAIN_LOCK)
|
||||
|
||||
var flag = false
|
||||
for (idx in 0..LOCKS_SIZE) {
|
||||
var lock = (LOCKS+idx).read()
|
||||
if (lock.is_active == true && lock.i2c_fd == fd) {
|
||||
_mutex_lock(lock.lock)
|
||||
(LOCKS+idx).write(lock)
|
||||
flag = true
|
||||
break
|
||||
}
|
||||
}
|
||||
|
||||
_mutex_unlock(MAIN_LOCK)
|
||||
if (flag) { return }
|
||||
throw NoneValueException("i2c lock nor found")
|
||||
}
|
||||
}
|
||||
|
||||
/* -------- 为一个I2C锁具解锁 -------- */
|
||||
@C
|
||||
func I2cLock_unlock(fd: Int32): Unit {
|
||||
unsafe {
|
||||
if (MAIN_LOCK.isNull()) {
|
||||
throw NoneValueException("no i2c lock")
|
||||
}
|
||||
_mutex_lock(MAIN_LOCK)
|
||||
|
||||
var flag = false
|
||||
for (idx in 0..LOCKS_SIZE) {
|
||||
var lock = (LOCKS+idx).read()
|
||||
if (lock.is_active == true && lock.i2c_fd == fd) {
|
||||
_mutex_unlock(lock.lock)
|
||||
(LOCKS+idx).write(lock)
|
||||
flag = true
|
||||
break
|
||||
}
|
||||
}
|
||||
_mutex_unlock(MAIN_LOCK)
|
||||
if (flag) {return}
|
||||
throw NoneValueException("i2c lock nor found")
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* -------- 原生I2C通信类--------
|
||||
* INFO(20260521-qing): 直接调用glibc的接口 */
|
||||
public class I2C_Orig {
|
||||
private var fd: Int32 = -1
|
||||
private var path: String
|
||||
public init (path: String) {
|
||||
this.path = path
|
||||
let s1 = unsafe {
|
||||
LibC.mallocCString(path) }
|
||||
fd = unsafe { openI2cDev(s1) }
|
||||
}
|
||||
|
||||
public func _close(): Unit {
|
||||
unsafe { closeI2cDev(fd) }
|
||||
}
|
||||
|
||||
public func setAddr(addr: UInt8) {
|
||||
unsafe { setI2cAddr(fd, addr) }
|
||||
}
|
||||
|
||||
public func writeReg(reg: UInt8, val:UInt8): Unit {
|
||||
unsafe { writeI2cReg(fd, reg, val) }
|
||||
}
|
||||
|
||||
public func readReg(reg: UInt8): Int8 {
|
||||
var value = unsafe { readI2cReg(fd, reg) }
|
||||
return value
|
||||
}
|
||||
|
||||
/*--------获取文件句柄--------*/
|
||||
public func getFd(): Int32 {
|
||||
return fd
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
/* -------- I2C设备异常 -------- */
|
||||
open private class I2cExcept <: Exception {
|
||||
public I2cExcept(dev: String, msg: String) {
|
||||
super('${dev}: ${msg}') }
|
||||
}
|
||||
|
||||
/* -------- 打开I2C设备失败 -------- */
|
||||
private class I2cOpenedFailed <: I2cExcept {
|
||||
public I2cOpenedFailed(dev: String) {
|
||||
super(dev, "failed to open i2c device") }
|
||||
}
|
||||
|
||||
/* -------- 关闭I2C设备失败 -------- */
|
||||
private class I2cClosedFailed <: I2cExcept {
|
||||
public I2cClosedFailed(dev: String) {
|
||||
super(dev, "failed to close i2c device") }
|
||||
}
|
||||
|
||||
/* -------- I2C设置地址失败 -------- */
|
||||
private class I2cSetAddressFailed <: I2cExcept {
|
||||
public I2cSetAddressFailed(dev: String) {
|
||||
super(dev, "failed to open i2c device") }
|
||||
}
|
||||
|
||||
/* -------- I2C写从机I2C失败 -------- */
|
||||
private class I2cWriteRegFailed <: I2cExcept {
|
||||
public I2cWriteRegFailed(dev: String) {
|
||||
super(dev, "failed to write i2c reg") }
|
||||
}
|
||||
|
||||
/* -------- I2C读从机I2C失败 -------- */
|
||||
private class I2cReadRegFailed <: I2cExcept {
|
||||
public I2cReadRegFailed(dev: String) {
|
||||
super(dev, "failed to read i2c reg") }
|
||||
}
|
||||
|
||||
/* -------- 未知I2C异常 -------- */
|
||||
private class I2cUnknowExp <: I2cExcept {
|
||||
public I2cUnknowExp(dev: String, msg: String) {
|
||||
super(dev, msg) }
|
||||
}
|
||||
@@ -0,0 +1,243 @@
|
||||
/* ----------------
|
||||
* MPU6050陀螺仪与加速度计模块
|
||||
* -
|
||||
* TODO(20260516-qing): 陀螺仪需要卡尔曼滤波、通过四元数等方式获取姿态角
|
||||
* - 所以应该使用一个C/C++线程来持续地采样，并且留一个接口进行获取
|
||||
*/
|
||||
|
||||
package bot4cj.hd
|
||||
import std.time.*
|
||||
import std.math.*
|
||||
import pthread4cj.*
|
||||
import bot4cj.math.*
|
||||
|
||||
const ADDR_0: UInt8 = 0x68 /* 默认地址 */
|
||||
const ADDR_1: UInt8 = 0x68 /* 地址2 */
|
||||
const PWR_MGMT_1: UInt8 = 0x6B /* 电源管理寄存器1 */
|
||||
const PWR_NOSLEEP: UInt8 = 0x00 /* 电源管理 - 非睡眠模式 */
|
||||
const GYRO_CONF_REG: UInt8 = 0x1B /* 陀螺仪配置寄存器 */
|
||||
const GYRO_FS_500DPS: UInt8 = 0x08 /* 陀螺仪量程 - 正负500度/秒 */
|
||||
const GYRO_REG_X_HIGH: UInt8 = 0x43 /* x - 陀螺仪横轴 高字节 */
|
||||
const GYRO_REG_X_LOW: UInt8 = 0x44
|
||||
const GYRO_REG_Y_HIGH: UInt8 = 0x45 /* y */
|
||||
const GYRO_REG_Y_LOW: UInt8 = 0x46
|
||||
const GYRO_REG_Z_HIGH: UInt8 = 0x47 /* z */
|
||||
const GYRO_REG_Z_LOW: UInt8 = 0x48
|
||||
const ACCEL_CONF_REG: UInt8 = 0x1C /* 加速度计配置寄存器 */
|
||||
const ACCEL_FS_8g: UInt8 = 0x10 /* 加速度计量程 正负8g */
|
||||
const ACCEL_REG_X_HIGH: UInt8 = 0x3B
|
||||
const ACCEL_REG_X_LOW: UInt8 = 0x3C
|
||||
const ACCEL_REG_Y_HIGH: UInt8 = 0x3D
|
||||
const ACCEL_REG_Y_LOW: UInt8 = 0x3E
|
||||
const ACCEL_REG_Z_HIGH: UInt8 = 0x3F
|
||||
const ACCEL_REG_Z_LOW: UInt8 = 0x40
|
||||
|
||||
/* -------- MPU6050传感器数据 -------- */
|
||||
@C
|
||||
public struct MPU6050_sensorData {
|
||||
public var accel_x: Int16 = 0 /* 加速度计 */
|
||||
public var accel_y: Int16 = 0
|
||||
public var accel_z: Int16 = 0
|
||||
public var gyro_x: Int16 = 0 /* 陀螺仪 */
|
||||
public var gyro_y: Int16 = 0
|
||||
public var gyro_z: Int16 = 0
|
||||
public func print() { /* 格式化打印 */
|
||||
println("[accel x, y, z] ${accel_x}, ${accel_y}, ${accel_z}")
|
||||
println("[gyro x, y, z] ${gyro_x}, ${gyro_y}, ${gyro_z}")
|
||||
println("--------------------")
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* -------- 设置MPU6050 -------- */
|
||||
@C
|
||||
func mpu6050_setup(i2c_fd: Int32, addr: UInt8) {
|
||||
unsafe {
|
||||
I2cLock_lock(i2c_fd)
|
||||
setI2cAddr(i2c_fd, addr);
|
||||
sleep(Duration.millisecond * 1)
|
||||
writeI2cReg(i2c_fd, PWR_MGMT_1, PWR_NOSLEEP); /* 退出睡眠 */
|
||||
writeI2cReg(i2c_fd, GYRO_CONF_REG, GYRO_FS_500DPS); /* ±500°/s */
|
||||
writeI2cReg(i2c_fd, ACCEL_CONF_REG, ACCEL_FS_8g); /* ±8g */
|
||||
I2cLock_unlock(i2c_fd)
|
||||
sleep(Duration.millisecond * 100); /* 等待稳定 */
|
||||
}
|
||||
}
|
||||
|
||||
@C
|
||||
/* -------- 读取 16 位传感器数据（带符号）--------
|
||||
* FIXME: 是否能够正确处理符号位 */
|
||||
func read16BitReg(i2c_fd: Int32, high: UInt8, low: UInt8): Int16 {
|
||||
unsafe {
|
||||
let hh: Int16 = Int16(readI2cReg(i2c_fd, high))
|
||||
let ll: Int16 = Int16(readI2cReg(i2c_fd, low))
|
||||
return (hh << 8) | ll
|
||||
}
|
||||
}
|
||||
|
||||
/* -------- 读取传感器数据 -------- */
|
||||
@C
|
||||
func mpu6050_readSensor(i2c_fd: Int32, addr: UInt8) {
|
||||
var data = MPU6050_sensorData()
|
||||
|
||||
unsafe {
|
||||
I2cLock_lock(i2c_fd)
|
||||
setI2cAddr(i2c_fd, addr)
|
||||
data.accel_x = read16BitReg(i2c_fd, ACCEL_REG_X_HIGH, ACCEL_REG_X_LOW);
|
||||
data.accel_y = read16BitReg(i2c_fd, ACCEL_REG_Y_HIGH, ACCEL_REG_Y_LOW);
|
||||
data.accel_z = read16BitReg(i2c_fd, ACCEL_REG_Z_HIGH, ACCEL_REG_Z_LOW);
|
||||
data.gyro_x = read16BitReg(i2c_fd, GYRO_REG_X_HIGH, GYRO_REG_X_LOW);
|
||||
data.gyro_y = read16BitReg(i2c_fd, GYRO_REG_Y_HIGH, GYRO_REG_Y_LOW);
|
||||
data.gyro_z = read16BitReg(i2c_fd, GYRO_REG_Z_HIGH, GYRO_REG_Z_LOW);
|
||||
I2cLock_unlock(i2c_fd)
|
||||
}
|
||||
return data
|
||||
}
|
||||
|
||||
|
||||
/* -------- 计算PITCH角 -------- */
|
||||
//@C
|
||||
//func mpu6050_calcPitch()
|
||||
|
||||
/* -------- mpu6050主线程 --------
|
||||
* TODO(20260525-qing): 使用互补滤波就够用了 */
|
||||
const RAD_TO_DEG = 57.2956f32 /* ---- 弧度转换为角度的系数（180/π ≈ 57.2956） ---- */
|
||||
//foreign func pthread_testcancel(): Unit
|
||||
@C
|
||||
func mpu6050_main(args: CPointer<Unit>): CPointer<Unit> {
|
||||
|
||||
/* -------- 获取传入线程的参数 -------- */
|
||||
let param = unsafe{ CPointer<mpu6050_th_param>(args).read() }
|
||||
|
||||
/* -------- 为加速度计准备滤波器组 -------- */
|
||||
var kalman_accel_x = extKalman(20.0f32, 200.0f32)
|
||||
var kalman_accel_y = extKalman(20.0f32, 200.0f32)
|
||||
var kalman_accel_z = extKalman(20.0f32, 200.0f32)
|
||||
|
||||
/* -------- 为陀螺仪准备滤波器组 -------- */
|
||||
var kalman_gyro_x = extKalman(20.0f32, 200.0f32)
|
||||
var kalman_gyro_y = extKalman(20.0f32, 200.0f32)
|
||||
var kalman_gyro_z = extKalman(20.0f32, 200.0f32)
|
||||
|
||||
|
||||
while(true) {
|
||||
|
||||
println("I am running. i2c_fd: ${param.i2c_fd}, addr: ${param.addr}")
|
||||
//unsafe { _mutex_lock(mutex) }
|
||||
|
||||
/* -------- 获取传感器数据 -------- */
|
||||
var data = unsafe { mpu6050_readSensor(param.i2c_fd, param.addr) }
|
||||
|
||||
/* -------- 为加速度计执行滤波操作 -------- */
|
||||
let filtered_accel_x = kalman_accel_x.filter(Float32(data.accel_x))
|
||||
let filtered_accel_y = kalman_accel_y.filter(Float32(data.accel_y))
|
||||
let filtered_accel_z = kalman_accel_z.filter(Float32(data.accel_z))
|
||||
|
||||
/* -------- 为陀螺仪执行滤波操作 -------- */
|
||||
let filtered_gyro_x = kalman_gyro_x.filter(Float32(data.gyro_x))
|
||||
let filtered_gyro_y = kalman_gyro_y.filter(Float32(data.gyro_y))
|
||||
let filtered_gyro_z = kalman_gyro_z.filter(Float32(data.gyro_z))
|
||||
|
||||
|
||||
/* -------- 计算Pitch（俯仰角）和Roll（翻滚角） -------- */
|
||||
let roll = atan2(filtered_accel_y, filtered_accel_z) * RAD_TO_DEG
|
||||
let pitch = atan(-filtered_accel_x / sqrt(filtered_accel_y * filtered_accel_y + filtered_accel_z * filtered_accel_z)) * RAD_TO_DEG
|
||||
|
||||
let roll1 = atan(filtered_accel_y / sqrt(filtered_accel_x * filtered_accel_x + filtered_accel_z * filtered_accel_z)) * RAD_TO_DEG
|
||||
let pitch1 = atan2(-filtered_accel_x, filtered_accel_z) * RAD_TO_DEG
|
||||
|
||||
print("roll: ${roll}\t")
|
||||
println("pitch: ${pitch}")
|
||||
print("roll1: ${roll1}\t")
|
||||
println("pitch1: ${pitch1}")
|
||||
|
||||
//unsafe { _mutex_unlock(mutex) }
|
||||
sleep(Duration.millisecond * 10)
|
||||
|
||||
}
|
||||
|
||||
/* -------- 返回NULL指针以占位 -------- */
|
||||
return CPointer<Unit>()
|
||||
}
|
||||
|
||||
/* -------- mpu6050线程参数 --------
|
||||
* INFO(20260522-qing): mutex字段是一个锁的指针，需要在MPU6050的构造函数中获取资源
|
||||
* - 用于辅助别的线程获取值。 */
|
||||
@C
|
||||
struct mpu6050_th_param {
|
||||
public var i2c_fd: Int32 = 0 /* i2c文件句柄 */
|
||||
public var addr: UInt8 = 0 /* i2c设备地址 */
|
||||
public var mutex = CPointer<UInt8>() /* 用于辅助别的线程获取值的锁 */
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* -------- MPU6050陀螺仪、加速度计 --------
|
||||
* - 输入并持有一个I2C文件句柄 */
|
||||
public class MPU6050 {
|
||||
|
||||
/* -------- 主初始化器 --------
|
||||
* FIXME(20260517-qing): 由于gc的原因，我们可能会手动构造RAII，
|
||||
* - 将一部分初始化移动到别的函数 */
|
||||
public init(i2c_fd: Int32, addr: UInt8) {
|
||||
this.i2c_fd = i2c_fd
|
||||
this.addr = addr
|
||||
unsafe {
|
||||
param = LibC.malloc<mpu6050_th_param>()
|
||||
var tmp = mpu6050_th_param()
|
||||
tmp.i2c_fd = i2c_fd
|
||||
tmp.addr = addr
|
||||
tmp.mutex = LibC.malloc<UInt8>()
|
||||
param.write(tmp)
|
||||
}
|
||||
}
|
||||
|
||||
/* -------- 终结器 -------- */
|
||||
~init() {
|
||||
/* -------- 清除构造时申请的线程参数和锁 -------- */
|
||||
unsafe {
|
||||
var tmp = param.read()
|
||||
LibC.free(tmp.mutex)
|
||||
LibC.free(param)
|
||||
}
|
||||
}
|
||||
|
||||
/* -------- 初始化该模块 -------- */
|
||||
public func setup() {
|
||||
unsafe { mpu6050_setup(i2c_fd, addr) }
|
||||
}
|
||||
|
||||
/*----------------*/
|
||||
private var i2c_fd: Int32 /* -------- I2C设备文件句柄 -------- */
|
||||
private var addr: UInt8 /* -------- I2C设备地址 -------- */
|
||||
|
||||
|
||||
/* -------- 获取原始MPU6050传感器数据 -------- */
|
||||
public func readSensor(): MPU6050_sensorData {
|
||||
return unsafe { mpu6050_readSensor(i2c_fd, addr) }
|
||||
}
|
||||
|
||||
/* -------- mpu6050启动守护 --------
|
||||
* TODO(20260520-qing): 放进类中
|
||||
* --------------------------------- */
|
||||
var param: CPointer<mpu6050_th_param> /* -------- 线程参数必须能够持久保存 -------- */
|
||||
var pth: Option<Pth> = None /* -------- 线程对象 -------- */
|
||||
public func startDaemon() { /* -------- 启动线程 -------- */
|
||||
pth = Pth(mpu6050_main, CPointer<Unit>(param))
|
||||
}
|
||||
|
||||
/* -------- 停止线程 -------- */
|
||||
public func stopDaemon() {
|
||||
if (let Some(P) <- pth) {
|
||||
P.close() }
|
||||
}
|
||||
|
||||
/* -------- 等待线程终止 -------- */
|
||||
public func waitDaemon() {
|
||||
if (let Some(P) <- pth) {
|
||||
var ptr: CPointer<Unit> = CPointer<Unit>()
|
||||
P.join(CPointer<CPointer<Unit>>(CPointer<CPointer<Unit>>(ptr))) }
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,69 @@
|
||||
package bot4cj.hd
|
||||
import pthread4cj.*
|
||||
|
||||
/* pca9685舵机控制版 - bot4cj
|
||||
* TODO(20260515-qing): 由于对统一接口的需求，设计基于回调的虚拟化硬件
|
||||
* TODO(20260516-qing): 获取pca9685的中文或者英文参考资料
|
||||
* NOTE(20260618-qing): 该代码已经被Lua脚本所取代，留着备用说不定以后会删除。 */
|
||||
public class PCA9685 {
|
||||
private static const ADDR_0: UInt8 = 0x40 /* PCA9685的i2c地址 */
|
||||
private let i2c_fd: Int32
|
||||
private let addr: UInt8
|
||||
public init( i2c_fd: Int32,
|
||||
addr!: UInt8 = ADDR_0) { /* PCA9685硬件地址 */
|
||||
this.i2c_fd = i2c_fd
|
||||
this.addr = addr
|
||||
/* -------- 初始化PCA9685 --------
|
||||
* FIXME(20260517-qing): 在旧版机器人代码移除以后，移除这一段 */
|
||||
|
||||
}
|
||||
|
||||
/* -------- 设置PWM频率为50Hz（舵机标准周期20ms0） --------
|
||||
* TODO(20260516-qing): 不将数值写死 */
|
||||
public func setup() {
|
||||
unsafe {
|
||||
I2cLock_lock(i2c_fd)
|
||||
setI2cAddr(i2c_fd, addr);
|
||||
writeI2cReg(i2c_fd, 0x00, 0x20); /* 开启AI（地址自增）和关闭SLEEP */
|
||||
sleep(5000 * Duration.nanosecond); /* 等待振荡器启动 */
|
||||
/* FIXME: 这一条是不是重复了 */
|
||||
var freq: Float32 = 50.0;
|
||||
var prescale_val: Float32 = 25000000.0 / (4096.0 * freq) - 1.0; /* 25MHz主频 */
|
||||
var prescale: UInt8 = UInt8(prescale_val + 0.5);
|
||||
|
||||
writeI2cReg(i2c_fd, 0x00, 0x10); /* 进入SLEEP模式（允许改频率） */
|
||||
writeI2cReg(i2c_fd, 0xFE, prescale); /* 写入预分频值 */
|
||||
writeI2cReg(i2c_fd, 0x00, 0x20); /* 退出SLEEP */
|
||||
|
||||
I2cLock_unlock(i2c_fd)
|
||||
}
|
||||
sleep(5000 * Duration.nanosecond); /* 等待振荡器启动 */
|
||||
}
|
||||
|
||||
|
||||
/* -------- 设置指定通道的PWM信号（0-4095）-------- */
|
||||
public func setPWM(channel: UInt8, on: UInt16, off: UInt16) {
|
||||
var reg: UInt8 = 0x06 + 4 * channel; /* LED0 寄存器起始地址 */
|
||||
unsafe {
|
||||
writeI2cReg(i2c_fd, reg, UInt8(on & 0xFF));
|
||||
writeI2cReg(i2c_fd, reg + 1, UInt8(on >> 8));
|
||||
writeI2cReg(i2c_fd, reg + 2, UInt8(off & 0xFF));
|
||||
writeI2cReg(i2c_fd, reg + 3, UInt8(off >> 8));
|
||||
}
|
||||
return
|
||||
}
|
||||
|
||||
/* -------- 角度转PWM值（0°~180° → 0.5ms~2.5ms脉宽） -------- */
|
||||
public func setAngle(channel: UInt8, angle: Float32) {
|
||||
const min: Float32 = 0.5; /* 0°对应0.5ms */
|
||||
const max: Float32 = 2.5; /* 180°对应2.5ms */
|
||||
let width: Float32 = min + (max - min) * (angle / 180.0);
|
||||
let off_val: UInt16 = UInt16(width * 4096.0 / 20.0); /* 20ms 周期 */
|
||||
unsafe {
|
||||
I2cLock_lock(i2c_fd)
|
||||
setI2cAddr(i2c_fd, this.addr);
|
||||
this.setPWM(channel, 0, off_val); /* ON=0, OFF=计算值 */
|
||||
I2cLock_unlock(i2c_fd)
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,30 @@
|
||||
package bot4cj.hd
|
||||
import std.math.*
|
||||
/* -------- 舵机类 --------
|
||||
* 含最大值、最小值、漂移值三个调整项
|
||||
* 以及一个设置角度回调函数
|
||||
*
|
||||
* FIXME(20260618-qing): 在构造函数中设置角度最大值 */
|
||||
public class Servo {
|
||||
private var max: Float32
|
||||
private var min: Float32
|
||||
private var offset: Float32
|
||||
private var callback_setAngle: (Float32) -> Unit
|
||||
private static let MAX_ANGLE: Float32 = 180.0f32
|
||||
public init(max!: Float32, min!: Float32, offset!: Float32,
|
||||
callback_setAngle!: (Float32) -> Unit) {
|
||||
this.max =max
|
||||
this.min = min
|
||||
this.offset = offset
|
||||
this.callback_setAngle = callback_setAngle
|
||||
}
|
||||
|
||||
public func setAngle(angle: Float32) {
|
||||
var a = angle
|
||||
a += offset /* ---- 加上漂移值 ---- */
|
||||
if (a > abs(max)) { a = abs(max)}
|
||||
if (a < abs(min)) { a = abs(min)}
|
||||
if (max < 0.0f32) { a = MAX_ANGLE - a }
|
||||
callback_setAngle(a)
|
||||
}
|
||||
}
|
||||
+16
@@ -0,0 +1,16 @@
|
||||
macro package bot4cj.hd.extern
|
||||
|
||||
import std.fs.*
|
||||
import std.io.*
|
||||
import std.ast.*
|
||||
|
||||
public macro ExternHardware(attrTokens: Tokens, inputTokens: Tokens): Tokens {
|
||||
let expr = LitConstExpr(attrTokens)
|
||||
let file = File(expr.literal.value, OpenMode.Read)
|
||||
let bytes = readToEnd(file)
|
||||
file.close()
|
||||
let sb = StringBuilder()
|
||||
sb.appendFromUtf8(bytes)
|
||||
let code = sb.toString()
|
||||
return cangjieLex(code)
|
||||
}
|
||||
@@ -0,0 +1,138 @@
|
||||
package bot4cj.hd
|
||||
import ctype4cj.*
|
||||
|
||||
const FBIOGET_VSCREENINFO = 0x4602u64
|
||||
const FBIOGET_FSCREENINFO = 0x4600u64
|
||||
const PROT_WRITE: C_Int = 0x2
|
||||
const MAP_SHARED: C_Int = 0x01
|
||||
|
||||
/* --- 定义在I2C.cj里了 ---*/
|
||||
//foreign func ioctl(fd: C_Int, request: C_ULong, ...): C_Int
|
||||
/* ---- 内存映射 ---- */
|
||||
public type off_t = C_LongLong
|
||||
foreign func mmap(addr: CPointer<Unit>, length: UIntNative, prot: C_Int, flags: C_Int, fd: C_Int, offset: off_t): CPointer<Unit>
|
||||
|
||||
/* -------- 打开framebuffer帧缓冲设备 -------- */
|
||||
@C
|
||||
public func fb_open(fb_dev: CString): C_Int {
|
||||
return unsafe { libc_open(fb_dev, O_RDWR) }
|
||||
}
|
||||
|
||||
/* -------- 帧缓冲区固定参数 -------- */
|
||||
@C
|
||||
public struct fb_fix_screeninfo {
|
||||
var id: VArray<C_Char, $16> /* 设备驱动名称字符串，比如"TT Builtin" */
|
||||
var smem_start: C_ULong = 0 /* 帧缓冲内存（显存）的物理起始地址 */
|
||||
/* (physical address) */
|
||||
var smem_len: UInt32 = 0 /* 帧缓冲内存的总长度，单位字节 */
|
||||
var type_: UInt32 = 0 /* 显存类型（fb存储布局）see FB_TYPE_* */
|
||||
var type_aux: UInt32 = 0 /* 显存附加类型 */
|
||||
var visual: UInt32 = 0 /* 色彩显示模式 see FB_VISUAL_* */
|
||||
var xpanstep: UInt16 = 0 /* zero if no hardware panning */
|
||||
var ypanstep: UInt16 = 0 /* zero if no hardware panning */
|
||||
var ywrapstep: UInt16 = 0 /* zero if no hardware ywrap */
|
||||
var line_length: UInt32 = 0 /* 显示屏上一行像素数据所占用的字节数 */
|
||||
var mmio_start: C_ULong = 0 /* 内存映射I/O的物理起始地址 */
|
||||
/* (physical address) */
|
||||
var mmio_len: UInt32 = 0 /* 内存映射的区域长度，单位字节 */
|
||||
var accel: UInt32 = 0 /* 硬件加速类型 */
|
||||
/* specific chip/card we have */
|
||||
var capabilities: UInt16 = 0 /* see FB_CAP_* */
|
||||
var reserved: VArray<UInt16, $2> /* 保留字段，用于未来扩展，确保兼容 */
|
||||
|
||||
public init() {
|
||||
id = VArray<C_Char, $16> (repeat: 0)
|
||||
reserved = VArray<UInt16, $2> (repeat: 0)
|
||||
}
|
||||
}
|
||||
|
||||
/* 颜色字段偏移量的解释：所有偏移量均从右侧开始计算，
|
||||
* 位于一个“像素”值内部，该像素值的宽度正好等于 'bits_per_pixel'
|
||||
* （也就是说：你可以将偏移量作为 << 的右操作数使用）。
|
||||
* 之后的像素就是一个比特流，会原封不动地写入显存。
|
||||
*
|
||||
* 对于伪彩色：所有颜色分量的偏移量和长度应该相同。
|
||||
* 偏移量指定了调色板索引的最低有效位在像素值中的位置。
|
||||
* 长度表示可用的调色板条目数量（即条目数 = 1 << 长度）。
|
||||
*/
|
||||
@C
|
||||
public struct fb_bitfield {
|
||||
var offset: UInt32 = 0 /* 位域的起始位置（从右侧算起）*/
|
||||
var length: UInt32 = 0 /* 位域的长度（比特数） */
|
||||
var msb_right: UInt32 = 0 /* != 0 : 最高有效位在右侧 */
|
||||
/* （即低位在右） */
|
||||
}
|
||||
|
||||
/* -------- 帧缓冲区可变参数 -------- */
|
||||
@C
|
||||
public struct fb_var_screeninfo {
|
||||
var xres: UInt32 = 0 /* visible resolution */
|
||||
var yres: UInt32 = 0
|
||||
var xres_virtual: UInt32 = 0 /* virtual resolution */
|
||||
var yres_virtual: UInt32 = 0
|
||||
var xoffset: UInt32 = 0 /* offset from virtual to visible */
|
||||
var yoffset: UInt32 = 0 /* resolution */
|
||||
|
||||
var bits_per_pixel: UInt32 = 0 /* guess what */
|
||||
var grayscale: UInt32 = 0 /* 0 = color, 1 = grayscale, */
|
||||
/* >1 = FOURCC */
|
||||
var red: fb_bitfield /* bitfield in fb mem if true color, */
|
||||
var green: fb_bitfield /* else only length is significant */
|
||||
var blue: fb_bitfield
|
||||
var transp: fb_bitfield /* transparency */
|
||||
|
||||
var nonstd: UInt32 = 0 /* != 0 Non standard pixel format */
|
||||
|
||||
var activate: UInt32 = 0 /* see FB_ACTIVATE_* */
|
||||
|
||||
var height: UInt32 = 0 /* height of picture in mm */
|
||||
var width: UInt32 = 0 /* width of picture in mm */
|
||||
|
||||
var accel_flags: UInt32 = 0 /* (OBSOLETE) see fb_info.flags */
|
||||
|
||||
/* Timing: All values in pixclocks, except pixclock (of course) */
|
||||
var pixclock: UInt32 = 0 /* pixel clock in ps (pico seconds) */
|
||||
var left_margin: UInt32 = 0 /* time from sync to picture */
|
||||
var right_margin: UInt32 = 0 /* time from picture to sync */
|
||||
var upper_margin: UInt32 = 0 /* time from sync to picture */
|
||||
var lower_margin: UInt32 = 0
|
||||
var hsync_len: UInt32 = 0 /* length of horizontal sync */
|
||||
var vsync_len: UInt32 = 0 /* length of vertical sync */
|
||||
var sync: UInt32 =0 /* see FB_SYNC_* */
|
||||
var vmode: UInt32 = 0 /* see FB_VMODE_* */
|
||||
var rotate: UInt32 = 0 /* angle we rotate counter clockwise */
|
||||
var colorspace: UInt32 = 0 /* colorspace for FOURCC-based modes */
|
||||
var reserved: VArray<UInt32, $4>/* Reserved for future compatibility */
|
||||
|
||||
public init() {
|
||||
red = fb_bitfield()
|
||||
green = fb_bitfield()
|
||||
blue = fb_bitfield()
|
||||
transp = fb_bitfield()
|
||||
reserved = VArray<UInt32, $4> (repeat: 0)
|
||||
}
|
||||
}
|
||||
|
||||
/* ---- 获取帧缓冲区可变参数 ---- */
|
||||
@C
|
||||
public func fb_get_var_info(fd: C_Int) {
|
||||
var info = fb_var_screeninfo()
|
||||
unsafe { ioctl(fd, FBIOGET_VSCREENINFO, inout info) }
|
||||
return info
|
||||
}
|
||||
|
||||
/* ---- 获取帧缓冲区固定参数 ---- */
|
||||
@C
|
||||
public func fb_get_fix_info(fd: C_Int) {
|
||||
var info = fb_fix_screeninfo()
|
||||
unsafe { ioctl(fd, FBIOGET_FSCREENINFO, inout info) }
|
||||
return info
|
||||
}
|
||||
|
||||
/* ---- 内存映射 ---- */
|
||||
@C
|
||||
public func fb_mmap(addr: CPointer<Unit>, length: UIntNative, fd: C_Int, offset: off_t): CPointer<Unit>
|
||||
{
|
||||
let buf = unsafe { mmap(addr, length, PROT_WRITE, MAP_SHARED, fd, offset) }
|
||||
return buf
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
package bot4cj.hd
|
||||
import std.time.*
|
||||
//foreign func play_frequency(frequency: Int32, duration_ms: Int32, sample_rate: Int32): Bool
|
||||
//foreign func play_frequency(frequency: Float32, duration_ms: Int32, volume: Float64, channels: Int32, sample_rate: Int32): Bool
|
||||
|
||||
/* -------- 扬声器类 --------
|
||||
* TODO: 一次打开设备，一段时间后再关闭 */
|
||||
public class Spk {
|
||||
private var channels: Int32
|
||||
private var sample_rate: Int32
|
||||
public init(channels!: Int32, sample_rate!: Int32) {
|
||||
this.channels = channels
|
||||
this.sample_rate = sample_rate
|
||||
}
|
||||
|
||||
/* -------- 控制扬声器发声 -------- */
|
||||
public func play(frequency: Float32, volume: Float64, durations_ms: Int32): Bool {
|
||||
//unsafe { play_frequency(frequency, durations_ms, volume, channels, sample_rate) }
|
||||
sleep(Duration.millisecond * Int64(durations_ms))
|
||||
return true
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user