移植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,152 @@
|
||||
/*
|
||||
* i2c - 用户界面
|
||||
*
|
||||
* 这里已经提供了I2C通用接口，可以将上层代码移动到仓颉下面
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <fcntl.h>
|
||||
#ifdef __linux__
|
||||
#include <unistd.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <linux/i2c-dev.h> /* 其中宏的取值可能具有平台特异性 */
|
||||
#include <i2c/smbus.h>
|
||||
#endif
|
||||
|
||||
#include "bot4cj.h" /* 有没有可能在仓颉代码中解析C头文件取得宏 */
|
||||
|
||||
|
||||
/*
|
||||
* 初始化i2c设备
|
||||
*
|
||||
* 动态指定设备文件
|
||||
*
|
||||
* 返回打开的文件句柄或者错误码
|
||||
*/
|
||||
#ifdef __linux__
|
||||
int16_t I2C_init(const char* filepath) {
|
||||
int16_t fd;
|
||||
if ((fd = open(filepath, O_RDWR)) < 0) {
|
||||
perror("Failed to open I2C bus");
|
||||
return BOT4CJ_I2C_OPENED_FAILED;
|
||||
}
|
||||
else {
|
||||
return fd;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#ifdef _WIN64
|
||||
int16_t I2C_init(const char* filepath) {
|
||||
return BOT4CJ_I2C_OPENED_FAILED;
|
||||
}
|
||||
#endif
|
||||
|
||||
/* 释放I2C设备文件 */
|
||||
#ifdef __linux__
|
||||
void I2C_free(int16_t fd) {
|
||||
close(fd);
|
||||
}
|
||||
#endif
|
||||
#ifdef _WIN64
|
||||
void I2C_free(int16_t fd) {
|
||||
/* Windows平台下无需操作 */
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
/* 设置I2C地址 */
|
||||
#ifdef __linux__
|
||||
int I2C_setAddr(int16_t fd, uint8_t addr) {
|
||||
if (ioctl(fd, I2C_SLAVE, addr) < 0) {
|
||||
perror("Failed to set I2C address");
|
||||
return BOT4CJ_I2C_ERR;
|
||||
}
|
||||
else {
|
||||
return BOT4CJ_I2C_OK;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#ifdef _WIN64
|
||||
int I2C_setAddr(int16_t fd, uint8_t addr) {
|
||||
return BOT4CJ_I2C_ERR;
|
||||
}
|
||||
#endif
|
||||
|
||||
/* 写寄存器 */
|
||||
#ifdef __linux__
|
||||
int I2C_writeReg(int16_t fd, uint8_t reg, uint8_t val) {
|
||||
/*uint8_t buf[] = {reg, val};
|
||||
if (write(fd, buf, 2) != 2) {
|
||||
perror("I2C write error");
|
||||
return BOT4CJ_I2C_ERR;
|
||||
}
|
||||
else {
|
||||
return BOT4CJ_I2C_OK;
|
||||
}*/
|
||||
|
||||
return i2c_smbus_write_byte_data(fd, reg, val);
|
||||
}
|
||||
#endif
|
||||
#ifdef _WIN64
|
||||
int I2C_writeReg(int16_t fd, uint8_t reg, uint8_t val) {
|
||||
return BOT4CJ_I2C_ERR;
|
||||
}
|
||||
#endif
|
||||
|
||||
/* 读寄存器 失败返回0 */
|
||||
#ifdef __linux__
|
||||
uint8_t I2C_readReg(int16_t fd, uint8_t reg) {
|
||||
|
||||
uint8_t value;
|
||||
|
||||
/*if (write(fd, ®, 1) != 1) {
|
||||
perror("I2C read error");
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (read(fd, &value, 1)!= 1) {
|
||||
perror("I2C read error");
|
||||
return 0;
|
||||
}*/
|
||||
|
||||
//return value;
|
||||
|
||||
int8_t a = i2c_smbus_read_byte_data(fd, reg);
|
||||
return *(uint8_t *)& a;
|
||||
}
|
||||
#endif
|
||||
#ifdef _WIN64
|
||||
uint8_t I2C_readReg(int16_t fd, uint8_t reg) {
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
/* 读寄存器 - 有符号
|
||||
* 失败返回0 */
|
||||
#ifdef __linux__
|
||||
int8_t I2C_readRegFlag(int16_t fd, uint8_t reg) {
|
||||
|
||||
int8_t value;
|
||||
|
||||
//if (write(fd, ®, 1) != 1) {
|
||||
//perror("I2C read error");
|
||||
//return 0;
|
||||
//}
|
||||
|
||||
//if (read(fd, &value, 1)!= 1) {
|
||||
//perror("I2C read error");
|
||||
//return 0;
|
||||
//}
|
||||
|
||||
//return value;
|
||||
|
||||
return i2c_smbus_read_byte_data(fd, reg);
|
||||
}
|
||||
#endif
|
||||
#ifdef _WIN64
|
||||
int8_t I2C_readRegFlag(int16_t fd, uint8_t reg) {
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,516 @@
|
||||
#include <random>
|
||||
#include <algorithm>
|
||||
#include "Matrix.hh"
|
||||
|
||||
namespace qing {
|
||||
|
||||
template <typename T>
|
||||
Matrx<T>::Matrx(): row(0), col(0) {}
|
||||
|
||||
template <typename T>
|
||||
Matrx<T>::Matrx(const uint64_t row, const uint64_t col)
|
||||
: row(row), col(col) { /* 该构造函数用于从头开始构造 */
|
||||
arr = std::vector<T>(col * row);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Matrx<T>::Matrx(const uint64_t row, const uint64_t col, const std::vector<T>& src)
|
||||
: row(row), col(col) { /* 该构造函数用于通过已有的vector进行构造 */
|
||||
arr = src;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
const T& Matrx<T>::get(uint64_t row, uint64_t col) const { /* 访问器 - 访问矩阵元素 */
|
||||
return arr[col * this->row + row];
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
T& Matrx<T>::witch(uint64_t row, uint64_t col) { /* 访问器 - 访问矩阵元素 */
|
||||
return arr[col * this->row + row];
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
void Matrx<T>::set(uint64_t row, uint64_t col, T v) { /* 设置矩阵元素 */
|
||||
arr[col * this->row + row] = v;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
const std::vector<T>& Matrx<T>::get_data() const{ /* 访问器 - 用于获取原始数据 */
|
||||
return arr;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
const uint64_t Matrx<T>::get_col() const { /* 获取列数 */
|
||||
return col;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
const uint64_t Matrx<T>::get_row() const { /* 获取行数 */
|
||||
return row;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
bool Matrx<T>::has_same_shape(const Matrx<T>& other) { /* 两个矩阵是否具有相同形状 */
|
||||
return other.get_col() == this->get_col() && other.get_row() == this->get_row();
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
bool Matrx<T>::has_dot_shape(const Matrx<T>& other) { /* 具有矩阵乘法的形状 */
|
||||
return this->get_col() == other.get_row();
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
void Matrx<T>::assert_same_shape(const Matrx<T>& other) { /* 断言两个矩阵具有相同形状 */
|
||||
if (!has_same_shape(other))
|
||||
throw std::invalid_argument("The shapes of matrix are not same.");
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
void Matrx<T>::assert_dot_shape(const Matrx<T>& other) { /* 断言两个矩阵具有点积前置形状 */
|
||||
if (!has_dot_shape(other)) {
|
||||
throw std::invalid_argument("The shapes of matrix can not be dot.");
|
||||
}
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Matrx<T> Matrx<T>::operator+(const Matrx<T>& other) { /* 加法操作符重定义 - 矩阵加法 */
|
||||
//assert_same_shape(other); /* 检查两个矩阵形状是否一致 */
|
||||
std::vector<T> v0 = this->get_data();
|
||||
std::vector<T> vec = other.get_data();
|
||||
for (auto it = v0.begin(), it_or = vec.begin(); it != v0.end(); ++it, ++it_or )
|
||||
*it += *it_or;
|
||||
return Matrx<T>{this->get_row(), this->get_col(), v0};
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Matrx<T> Matrx<T>::operator+(T& item) { /* 矩阵加法广播 */
|
||||
auto len = this->get_data().size();
|
||||
auto v0 = std::vector<T> (len, item);
|
||||
auto matrix = (*this) + Matrx(get_row(), get_col(), v0);
|
||||
|
||||
//auto matrix = *this;
|
||||
//for (auto it = arr.begin(); it != arr.end(); ++it)
|
||||
// *it += item;
|
||||
return matrix;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Matrx<T> Matrx<T>::operator-(const Matrx<T>& other) { /* 操作符重载 - 矩阵减法 */
|
||||
//assert_same_shape(other); /* 检查两个矩阵形状是否一致 */
|
||||
std::vector<T> v0 = this->get_data();
|
||||
std::vector <T> vec = other.get_data();
|
||||
for (auto it = v0.begin(), it_or = vec.begin(); it != v0.end(); ++it, ++it_or )
|
||||
*it -= *it_or;
|
||||
return Matrx{this->get_row(), this->get_col(), v0};
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Matrx<T> Matrx<T>::operator-(T& item) { /* 操作符重载 - 矩阵减法广播 */
|
||||
auto len = this->get_data().size();
|
||||
auto v0 = std::vector<T> (len, item);
|
||||
auto matrix = (*this) - Matrx(get_row(), get_col(), v0);
|
||||
|
||||
//Matrx<T> matrix = *this;
|
||||
//for (auto it = arr.begin(); it != arr.end(); ++it)
|
||||
// *it -= item;
|
||||
return matrix;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Matrx<T> Matrx<T>::dot(const Matrx<T>& other) { /* 操作符重载 - 矩阵乘法 */
|
||||
//assert_dot_shape(other); /* 它们的形状是否能够点积 */
|
||||
auto matrix = Matrx<T>(this->get_row(), other.get_col());
|
||||
for (uint64_t i=0; i<this->get_row(); ++i) /* 点积运算 */
|
||||
for (uint64_t j=0; j<other.get_col(); ++j) {
|
||||
T sum{0};
|
||||
for (uint64_t k=0; k<this->get_col(); ++k)
|
||||
sum += this->get(i, k) * other.get(k, j);
|
||||
matrix.witch(i, j) = sum;
|
||||
}
|
||||
return matrix;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Matrx<T> Matrx<T>::operator*(T& item) { /* 操作符重载 - 矩阵乘法广播 */
|
||||
std::vector<T> vec = this->get_data();
|
||||
for (auto it = vec.begin(); it != vec.end(); ++it)
|
||||
*it *= item;
|
||||
return Matrx<T>{this->get_row(), this->get_col(), vec};
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Matrx<T> Matrx<T>::operator*(const Matrx<T>& other) { /* 加法操作符重定义 - 矩阵加法 */
|
||||
//assert_same_shape(other); /* 检查两个矩阵形状是否一致 */
|
||||
std::vector<T> v0 = this->get_data();
|
||||
std::vector<T> vec = other.get_data();
|
||||
for (auto it = v0.begin(), it_or = vec.begin(); it != v0.end(); ++it, ++it_or )
|
||||
*it *= *it_or;
|
||||
return Matrx<T>{this->get_row(), this->get_col(), v0};
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Matrx<T> Matrx<T>::operator/(T& item) { /* 操作符重载 - 矩阵除法广播 */
|
||||
std::vector<T> vec = this->get_data();
|
||||
for (auto it = vec.begin(); it != vec.end(); ++it)
|
||||
*it /= item;
|
||||
return Matrx<T>(this->get_row(), this->get_col(), vec);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Matrx<T> Matrx<T>::operator/(const Matrx<T>& other) { /* 加法操作符重定义 - 矩阵加法 */
|
||||
//assert_same_shape(other); /* 检查两个矩阵形状是否一致 */
|
||||
std::vector<T> v0 = this->get_data();
|
||||
std::vector<T> vec = other.get_data();
|
||||
for (auto it = v0.begin(), it_or = vec.begin(); it != v0.end(); ++it, ++it_or )
|
||||
*it /= *it_or;
|
||||
return Matrx<T>{this->get_row(), this->get_col(), v0};
|
||||
}
|
||||
|
||||
template class Matrx<float>; /* 显式实例化 */
|
||||
template class Matrx<double>;
|
||||
|
||||
}
|
||||
|
||||
/* FIXME: 形状不合法时返回空指针 */
|
||||
using namespace qing;
|
||||
extern "C" {
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void* createMatrix(uint64_t row, uint64_t col) {
|
||||
return (void *) new Matrx<float>(row, col);
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void freeMatrix(void* M) {
|
||||
delete (Matrx<float>*) M;
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
float* witchMatrix(void *m, uint64_t row, uint64_t col) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
return &(M->witch(row, col));
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
float getMatrix(void *m, uint64_t row, uint64_t col) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
return M->get(row, col);
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void setMatrix(void *m, uint64_t row, uint64_t col, float v) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
M->set(row, col, v);
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
uint64_t getrowMatrix(void *m) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
return M->get_row();
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
uint64_t getcolMatrix(void *m) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
return M->get_col();
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
bool sameShapeMatrix(void *m, void *s) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
Matrx<float> *S = (Matrx<float>*) s;
|
||||
return M->has_same_shape(*S);
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
bool dotShapeMatrix(void *m, void *s) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
Matrx<float> *S = (Matrx<float>*) s;
|
||||
return M->has_dot_shape(*S);
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void* plusMatrix(void *m, void *s) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
Matrx<float> *S = (Matrx<float>*) s;
|
||||
auto a = new Matrx<float>((*M) + (*S));
|
||||
return (void *) a;
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void* plusBroadcastMatrix(void *m, float t) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
auto a = new Matrx<float> ((*M) + t);
|
||||
return (void*) a;
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void* subMatrix(void *m, void *s) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
Matrx<float> *S = (Matrx<float>*) s;
|
||||
auto a = new Matrx<float>((*M) - (*S));
|
||||
return (void *) a;
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void * subBroadcastMatrix(void *m, float t) {
|
||||
Matrx<float> *M = (Matrx<float> *) m;
|
||||
auto a = new Matrx<float> ((*M) - t);
|
||||
return (void *) a;
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void* multiMatrix(void *m, void *s) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
Matrx<float> *S = (Matrx<float>*) s;
|
||||
auto a = new Matrx<float>((*M) * (*S));
|
||||
return (void *) a;
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void* multiBroadcastMatrix(void *m, float t) {
|
||||
Matrx<float> *M = (Matrx<float> *) m;
|
||||
auto a = new Matrx<float>((*M) * t);
|
||||
return (void*) a;
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void* divMatrix(void *m, void *s) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
Matrx<float> *S = (Matrx<float>*) s;
|
||||
auto a = new Matrx<float>((*M) / (*S));
|
||||
return (void *) a;
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void* divBroadcastMatrix(void *m, float t) {
|
||||
Matrx<float> *M = (Matrx<float> *) m;
|
||||
auto a = new Matrx<float>((*M) / t);
|
||||
return (void*) a;
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void* dotMatrix(void *m, void *s) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
Matrx<float> *S = (Matrx<float>*) s;
|
||||
auto a = new Matrx<float>(M->dot(*S));
|
||||
return (void *) a;
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void* transposeMatrix(void *m) {
|
||||
Matrx<float> *M = (Matrx<float> *)m;
|
||||
auto N= new Matrx<float>(M->get_col(), M->get_row());
|
||||
for (uint64_t row = 0; row< N->get_row(); row++) {
|
||||
for (uint64_t col = 0; col < N->get_col(); col++) {
|
||||
N->witch(row, col) = M->get(col, row);
|
||||
}
|
||||
}
|
||||
return (void *)N;
|
||||
}
|
||||
|
||||
/* 正态分布Xavier初始化 */
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void* xavierNormalInit(uint64_t fan_in, uint64_t fan_out)
|
||||
{
|
||||
/* 构建空数组 */
|
||||
std::vector<float> weights(fan_in * fan_out);
|
||||
|
||||
/* 高质量的随机数生成器 */
|
||||
static std::mt19937 generator(std::random_device{}());
|
||||
|
||||
/* 计算标准差 */
|
||||
float sigma = std::sqrt(2.0f / (fan_in + fan_out));
|
||||
|
||||
/* 创建正态分布对象，均值为0，标准差为sigma */
|
||||
std::normal_distribution<float> distribution(0.0f, sigma);
|
||||
|
||||
/* 遍历权重矩阵，进行赋值 */
|
||||
uint64_t k=0;
|
||||
for (uint64_t i=0; i<fan_in; ++i)
|
||||
for (uint64_t j=0; j<fan_out; ++j)
|
||||
weights[k++] = distribution(generator);
|
||||
|
||||
/* 构建矩阵 */
|
||||
auto matrix = new Matrx<float>(fan_in, fan_out, weights);
|
||||
return (void *)matrix;
|
||||
}
|
||||
|
||||
/* 均匀分布Xavier初始化函数 */
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void* xavierUniformInit(uint64_t fan_in, uint64_t fan_out)
|
||||
{
|
||||
/* 构建空的数组 */
|
||||
auto weights = std::vector<float>(fan_in * fan_out);
|
||||
|
||||
/* 使用高质量的随机数生成器 */
|
||||
static std::mt19937 generator(std::random_device{}());
|
||||
|
||||
/* 计算区间范围：sqrt(2 / (fan_in + fan_out)) */
|
||||
float scale = std::sqrt(2.0f / (fan_in + fan_out));
|
||||
|
||||
/* 创建均匀分布对象，范围为[-scale, scale] */
|
||||
std::uniform_real_distribution<float> distribution(-scale, scale);
|
||||
|
||||
/* 遍历权重矩阵，进行赋值 */
|
||||
uint64_t k=0;
|
||||
for (uint64_t i=0; i<fan_in; ++i)
|
||||
for (uint64_t j=0; j<fan_out; ++j)
|
||||
weights[k++] = distribution(generator);
|
||||
|
||||
/* 构建矩阵对象 */
|
||||
auto matrix = new Matrx<float> (fan_in, fan_out, weights);
|
||||
return (void *)matrix;
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void* Sigmoid_do(void *m) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
std::vector<float> v = M->get_data();
|
||||
for (float& x: v) {
|
||||
x = 1.0 / (1.0 + std::exp(-x));
|
||||
}
|
||||
auto N = new Matrx<float> (M->get_row(), M->get_col(), v);
|
||||
return (void *)N;
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void* Sigmoid_dot(void *m) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
std::vector<float> v = M->get_data();
|
||||
for (float& o: v) {
|
||||
o = o * (1.0 - o);
|
||||
}
|
||||
auto N = new Matrx<float> (M->get_row(), M->get_col(), v);
|
||||
return (void *)N;
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void* ReLU_do(void *m) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
std::vector<float> v = M->get_data();
|
||||
for (float& x: v) {
|
||||
x = x > 0 ? x : 0;
|
||||
}
|
||||
auto N = new Matrx<float> (M->get_row(), M->get_col(), v);
|
||||
return (void *)N;
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void* ReLU_dot(void *m) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
std::vector<float> v = M->get_data();
|
||||
for (float& x: v) {
|
||||
x = x > 0 ? 1.0f : 0.0f;
|
||||
}
|
||||
auto N = new Matrx<float> (M->get_row(), M->get_col(), v);
|
||||
return (void *)N;
|
||||
}
|
||||
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void* Leaky_ReLU_do(void *m, float alpha) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
std::vector<float> v = M->get_data();
|
||||
for (float& x: v) {
|
||||
x = x > 0 ? x : alpha * x;
|
||||
}
|
||||
auto N = new Matrx<float> (M->get_row(), M->get_col(), v);
|
||||
return (void *)N;
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void* Leaky_ReLU_dot(void *m, float alpha) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
std::vector<float> v = M->get_data();
|
||||
for (float& x: v) {
|
||||
x = x > 0 ? 1.0 : alpha;
|
||||
}
|
||||
auto N = new Matrx<float> (M->get_row(), M->get_col(), v);
|
||||
return (void *)N;
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void* Tanh_do(void *m) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
std::vector<float> v = M->get_data();
|
||||
for (float& x: v) {
|
||||
x = std::tanh(x);
|
||||
}
|
||||
auto N = new Matrx<float> (M->get_row(), M->get_col(), v);
|
||||
return (void *)N;
|
||||
}
|
||||
|
||||
#ifdef _WIN64
|
||||
_declspec(dllexport)
|
||||
#endif
|
||||
void* Tanh_dot(void *m) {
|
||||
Matrx<float> *M = (Matrx<float>*) m;
|
||||
std::vector<float> v = M->get_data();
|
||||
for (float& x: v) {
|
||||
if (x > 20.0) x = 0.0; // tanh(x) ≈ 1, 导数≈0
|
||||
else if (x < -20.0) x = 0.0; // tanh(x) ≈ -1, 导数≈0
|
||||
else {
|
||||
float t = tanh(x);
|
||||
x = 1.0 - t * t;
|
||||
}
|
||||
}
|
||||
auto N = new Matrx<float> (M->get_row(), M->get_col(), v);
|
||||
return (void *)N;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
/*
|
||||
|
||||
___col___
|
||||
/ \
|
||||
+---------+ \
|
||||
| + + + + | | row - 第一维度
|
||||
| + + + + | |
|
||||
+---------+ /
|
||||
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <stdexcept>
|
||||
#include <cstdint>
|
||||
namespace qing {
|
||||
template <class T>
|
||||
class Matrx {
|
||||
public:
|
||||
Matrx();
|
||||
Matrx(const uint64_t row, const uint64_t col);
|
||||
Matrx(const uint64_t row, const uint64_t col, const std::vector<T>& src);
|
||||
const T& get(uint64_t row, uint64_t col) const;
|
||||
//-------------旧接口----------------
|
||||
T& witch(uint64_t row, uint64_t col);
|
||||
//-------------新接口----------------
|
||||
void set(uint64_t row, uint64_t col, T v);
|
||||
//-----------------------------------
|
||||
const std::vector<T>& get_data() const;
|
||||
const uint64_t get_col() const;
|
||||
const uint64_t get_row() const;
|
||||
bool has_same_shape(const Matrx<T>& other);
|
||||
bool has_dot_shape(const Matrx<T>& other);
|
||||
void assert_same_shape(const Matrx<T>& other);
|
||||
void assert_dot_shape(const Matrx<T>& other);
|
||||
Matrx<T> operator+(const Matrx<T>& other);
|
||||
Matrx<T> operator+(T& item);
|
||||
Matrx<T> operator-(const Matrx<T>& other);
|
||||
Matrx<T> operator-(T& item);
|
||||
Matrx<T> operator*(const Matrx<T>& other);
|
||||
Matrx<T> operator*(T& item);
|
||||
Matrx<T> operator/(T& item);
|
||||
Matrx<T> operator/(const Matrx<T>& other);
|
||||
Matrx<T> dot(const Matrx<T>& other);
|
||||
|
||||
void save(std::ostream& out) {
|
||||
out << row << " " << col;
|
||||
for (const auto& item : arr) {
|
||||
out << " ";
|
||||
out << item;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
uint64_t row, col;
|
||||
std::vector<T> arr;
|
||||
};
|
||||
}
|
||||
@@ -0,0 +1,21 @@
|
||||
/* I2C操作库 - PCA9685 - MPU6050 */
|
||||
#ifndef QING_I2C
|
||||
#define QING_I2C
|
||||
/* I2C */
|
||||
#define BOT4CJ_I2C_OK 0
|
||||
#define BOT4CJ_I2C_ERR 1
|
||||
#define BOT4CJ_I2C_OPENED_FAILED -1
|
||||
|
||||
#ifdef _WIN64
|
||||
#define DLLEXPORT __declspec(dllexport)
|
||||
#else
|
||||
#define DLLEXPORT
|
||||
#endif
|
||||
|
||||
DLLEXPORT int16_t I2C_init(const char* filepath);
|
||||
DLLEXPORT void I2C_free(int16_t fd);
|
||||
DLLEXPORT int I2C_setAddr(int16_t fd, uint8_t addr);
|
||||
DLLEXPORT int I2C_writeReg(int16_t fd, uint8_t reg, uint8_t val);
|
||||
DLLEXPORT uint8_t I2C_readReg(int16_t fd, uint8_t reg);
|
||||
DLLEXPORT int8_t I2C_readRegFlag(int16_t fd, uint8_t reg);
|
||||
#endif
|
||||
@@ -0,0 +1,80 @@
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
#include "fft.h"
|
||||
#include "Matrix.hh"
|
||||
using namespace qing;
|
||||
|
||||
|
||||
|
||||
void functf1d(float A[DN])
|
||||
{
|
||||
unsigned long int i;
|
||||
float dt, tt;
|
||||
float p = 0.1;
|
||||
float freq = 10.0; // 10Hz
|
||||
float fs = DN; // 采样率65536Hz
|
||||
|
||||
dt = 1.0 / fs;
|
||||
|
||||
for(i = 0; i < DN; i++)
|
||||
{
|
||||
tt = (float)i * dt;
|
||||
A[i] = sin(2.0 * M_PI * freq * tt) * p; // 正弦波
|
||||
//A[1][i] = 0.0; // 虚部为0
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 初始化FFT数组
|
||||
static void fft_init(float A[2][DN], Matrx<float>& M)
|
||||
{
|
||||
for (int i = 0; i < DN; i++) {
|
||||
float a = M.witch(i, 0);
|
||||
A[0][i] = a;
|
||||
A[1][i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// 计算FFT结果
|
||||
static void fft_result(std::vector<float>& R, float A[2][DN])
|
||||
{
|
||||
for (int i = 0; i < DN/2; i++) {
|
||||
float a = pow(A[0][i], 2) + pow(A[1][i], 2);
|
||||
R[i] = sqrt(a) / (DN/2);
|
||||
//std::cout << i << ":\t" << R[i] << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
extern "C" {
|
||||
|
||||
// 生成正弦波
|
||||
#ifdef _WIN64
|
||||
__declspec(dllexport) void* gen_wave()
|
||||
#else
|
||||
void* gen_wave()
|
||||
#endif
|
||||
{
|
||||
std::vector<float> A(DN);
|
||||
functf1d(A.data());
|
||||
return (void *)new Matrx<float>(DN, 1, A);
|
||||
}
|
||||
|
||||
// 开始FFT
|
||||
#ifdef _WIN64
|
||||
__declspec(dllexport) void* start_fft(void* M)
|
||||
#else
|
||||
void* start_fft(void* M)
|
||||
#endif
|
||||
{
|
||||
float A[2][DN];
|
||||
std::vector<float> R(N);
|
||||
fft_init(A, *(Matrx<float> *)M);
|
||||
//fft1(A, 1); // 复序列基2 FFT
|
||||
fft1r1(A, 1); // 单实序列
|
||||
fft_result(R, A);
|
||||
return (void*) new Matrx<float>(N/2, 1, R);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,29 @@
|
||||
#ifndef FFT_H
|
||||
#define FFT_H
|
||||
|
||||
#ifndef N
|
||||
#define N 32768
|
||||
#endif
|
||||
|
||||
#ifndef DN
|
||||
#define DN (N*2)
|
||||
#endif
|
||||
|
||||
#define eps 1.0e-9
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
void initial(void);
|
||||
void fft1(float A[2][N], int ifft);
|
||||
unsigned long int inverseq(unsigned long int i1,
|
||||
unsigned long int imax);
|
||||
void fft1r2(float A[2][N], float B[2][N], int ifft);
|
||||
void fft1r1(float A[2][DN], int ifft);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,231 @@
|
||||
/* fft1.c: fast Fourier transform base on 2 for one dimensional
|
||||
complex array, direct and inverse,
|
||||
This program is contributed by Jiang Yong, 2003. 1. */
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include "fft.h"
|
||||
static int ifirst=0, sign=-1;
|
||||
static unsigned M, jup[64], kup[64];
|
||||
static float pi, wr[N], wi[N];
|
||||
/********* initial **************/
|
||||
void initial(void)
|
||||
{
|
||||
unsigned long int i, j, ni;
|
||||
float unit, tt;
|
||||
ni = N;
|
||||
M = 0;
|
||||
while (ni > 1) { ni=ni/2; M=M+1; }
|
||||
ni = 1;
|
||||
for(j=0; j<M; j++) ni = ni * 2;
|
||||
if ((ni-N) != 0)
|
||||
{
|
||||
printf("N isn't compatible! (N => %i, ni -> %lu)\n", N, ni);
|
||||
exit(1);
|
||||
}
|
||||
kup[0] = N/2; /* kup[l] = 2 ^ (M - l - 1) */
|
||||
jup[0] =1;
|
||||
for (j=1;j<M;j++)
|
||||
{
|
||||
kup[j]=kup[j-1]/2;
|
||||
jup[j]=jup[j-1]*2;
|
||||
}
|
||||
pi=4.0*atan((float)1.0);
|
||||
unit=2.0*pi/((float)N);
|
||||
tt=0.0;
|
||||
for(i=0;i<N;i++)
|
||||
{
|
||||
wr[i] = cos(tt);
|
||||
wi[i] = sign * sin(tt);
|
||||
tt = tt + unit;
|
||||
}
|
||||
}
|
||||
/** ifft=1<-->FFT ** ifft=-1<-->IFFT **/
|
||||
void fft1(float A[2][N], int ifft)
|
||||
{
|
||||
unsigned long int l, k, i, j, jl0, jl0k, jl1k, reseq, ij;
|
||||
float wrc, wis, ar, ai, x[2][N];
|
||||
if (ifirst == 0) { initial(), ifirst=1; }
|
||||
if (ifft == -1)
|
||||
for(i=0;i<N;i++) A[1][i] = -A[1][i];
|
||||
for (l=0; l<M; l++)
|
||||
{
|
||||
for (j=0; j<jup[l]; j++)
|
||||
{
|
||||
reseq=inverseq(j, l);
|
||||
ij=kup[l]*reseq;
|
||||
jl0=2*kup[l]*j;
|
||||
wrc = wr[ij];
|
||||
wis = wi[ij];
|
||||
for (k=0; k<kup[l]; k++)
|
||||
{
|
||||
jl0k = jl0 + k;
|
||||
jl1k = jl0 + kup[l] + k;
|
||||
ar = A[0][jl1k] * wrc - A[1][jl1k] * wis;
|
||||
ai = A[0][jl1k] * wis + A[1][jl1k] * wrc;
|
||||
A[0][jl1k] = A[0][jl0k] - ar;
|
||||
A[1][jl1k] = A[1][jl0k] - ai;
|
||||
A[0][jl0k] = A[0][jl0k] + ar;
|
||||
A[1][jl0k] = A[1][jl0k] + ai;
|
||||
}
|
||||
}
|
||||
}
|
||||
for (i=0; i<N; i++)
|
||||
{
|
||||
j=inverseq(i, M);
|
||||
x[0][i] = A[0][j];
|
||||
x[1][i] = A[1][j];
|
||||
}
|
||||
for (i=0; i<N; i++)
|
||||
{
|
||||
A[0][i] = x[0][i];
|
||||
A[1][i] = x[1][i];
|
||||
}
|
||||
if (ifft == -1)
|
||||
for (i=0; i<N; i++)
|
||||
{
|
||||
A[0][i] = A[0][i]/((float)N);
|
||||
A[1][i] = -A[1][i]/((float)N);
|
||||
}
|
||||
}
|
||||
/*********************************/
|
||||
unsigned long int inverseq(unsigned long int i1,
|
||||
unsigned long int imax)
|
||||
{
|
||||
unsigned long int j, ii, i0, inv;
|
||||
i0 = i1;
|
||||
inv = 0;
|
||||
for (j=0; j < imax; j++)
|
||||
{
|
||||
ii=i0%2;
|
||||
inv=2*inv+ii;
|
||||
i0=(i0-ii)/2;
|
||||
}
|
||||
return (inv);
|
||||
}
|
||||
/* ifft=1 <--> FFT ** ifft=-1 <--> IFFT */
|
||||
void fft1r2(float A[2][N], float B[2][N], int ifft)
|
||||
{
|
||||
unsigned long int k, j;
|
||||
float norm, c[2][N];
|
||||
if (ifft != -1)
|
||||
{
|
||||
for (k=0; k<N; k++)
|
||||
{
|
||||
c[0][k]=A[0][k];
|
||||
c[1][k]=B[0][k];
|
||||
}
|
||||
fft1(c, 1);
|
||||
A[0][0] = c[0][0];
|
||||
A[1][0] = 0.0;
|
||||
B[0][0] = c[1][0];
|
||||
B[1][0] = 0.0;
|
||||
for (j=1; j<N; j++)
|
||||
{
|
||||
A[0][j] = 0.5 * (c[0][j] + c[0][N-j]);
|
||||
A[1][j] = 0.5 * (c[1][j] - c[1][N-j]);
|
||||
B[0][j] = 0.5 * (c[1][j] + c[1][N-j]);
|
||||
B[1][j] = -0.5 * (c[0][j] - c[0][N-j]);
|
||||
}
|
||||
} /* end if*/
|
||||
else
|
||||
{
|
||||
for (j=0; j<N/2; j++)
|
||||
/* check {xj}, {yj} is to real {Ak}, {Bk} */
|
||||
{
|
||||
if (j == 0) norm = 2.0 * fabs(A[1][0])
|
||||
+ 2.0 * fabs(B[1][0]);
|
||||
else norm = fabs(A[1][j]+A[1][N-j])
|
||||
+ fabs(A[0][j]-A[0][N-j])
|
||||
+ fabs(B[1][j]+B[1][N-j])
|
||||
+ fabs(B[0][j]-B[0][N-j]);
|
||||
if (norm > eps)
|
||||
{
|
||||
printf("The IDFT of the series isn't real!\n");
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
for (j=0; j<N; j++)
|
||||
{
|
||||
c[0][j]=A[0][j] -B[1][j];
|
||||
c[1][j]=A[1][j] +B[0][j];
|
||||
}
|
||||
fft1(c, -1);
|
||||
for (k=0; k<N; k++)
|
||||
{
|
||||
A[0][k] = c[0][k];
|
||||
B[0][k] = c[1][k];
|
||||
A[1][k] = 0.0;
|
||||
B[1][k] = 0.0;
|
||||
}
|
||||
} /* end else */
|
||||
}
|
||||
/* ifft = 1 <--> FFT ** ifft = -1 <--> IFFT */
|
||||
void fft1r1(float A[2][DN], int ifft)
|
||||
{
|
||||
unsigned long int j, k, dk;
|
||||
float norm, c[2][N], d[2][N], tt, hunit, cosj, sinj;
|
||||
hunit = 4.0 * atan(1.0) / (float)N;
|
||||
if ( ifft != -1)
|
||||
{
|
||||
for (k=0; k<N; k++)
|
||||
{
|
||||
dk = k + k;
|
||||
c[0][k] = A[0][dk];
|
||||
d[0][k] = A[0][dk+1];
|
||||
c[1][k] = 0.0;
|
||||
d[1][k] = 0.0;
|
||||
}
|
||||
fft1r2(c, d, 1);
|
||||
for (j = 0; j < N; j++)
|
||||
{
|
||||
tt = (float)j * hunit;
|
||||
cosj = cos(tt);
|
||||
sinj = sign * sin(tt);
|
||||
A[0][j] = c[0][j] + cosj * d[0][j] - sinj * d[1][j];
|
||||
A[1][j] = c[1][j] + cosj * d[1][j] + sinj * d[0][j];
|
||||
}
|
||||
A[0][N] = c[0][0] - d[0][0];
|
||||
A[1][N] = c[1][0] - d[1][0];
|
||||
for(j=1; j<N; j++)
|
||||
{
|
||||
A[0][DN-j] = A[0][j];
|
||||
A[1][DN-j] = -A[1][j];
|
||||
}
|
||||
} // end if
|
||||
|
||||
else
|
||||
{
|
||||
for (j = 0; j <N; j++) // check {xj} is to real {Ak}
|
||||
{
|
||||
if (j ==0) norm = 2.0 * fabs(A[1][0]);
|
||||
else norm = fabs(A[1][j]+A[1][DN-j])+
|
||||
fabs(A[0][j]-A[0][DN-j]);
|
||||
if (norm > eps)
|
||||
{
|
||||
printf("The IDFT of the series {xj} isn't real!\n");
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
for (j = 0; j <N; j++)
|
||||
{
|
||||
c[0][j]= 0.5 * (A[0][j]+A[0][N+j]);
|
||||
c[1][j]= 0.5 * (A[1][j]+A[1][N+j]);
|
||||
tt = (float) j * hunit;
|
||||
cosj = cos(tt);
|
||||
sinj = -sign*sin(tt);
|
||||
d[0][j]= 0.5 * (cosj * (A[0][j]-A[0][N+j])
|
||||
-sinj*(A[1][j] - A[1][N+j]));
|
||||
}
|
||||
fft1r2(c, d, -1);
|
||||
for (k=0; k<N; k++)
|
||||
{
|
||||
dk = k+k;
|
||||
A[0][dk]=c[0][k];
|
||||
A[0][dk+1]=d[0][k];
|
||||
A[1][dk]=0.0;
|
||||
A[1][dk+1]=0.0;
|
||||
}
|
||||
} // end else
|
||||
}
|
||||
|
||||
@@ -0,0 +1,63 @@
|
||||
/**
|
||||
* 一维卡尔曼滤波器
|
||||
* 使用时先定义一个kalman指针，然后调用kalmanCreate()创建一个滤波器
|
||||
* 每次读取到传感器数据后即可调用KalmanFilter()来对数据进行滤波
|
||||
* 使用示例
|
||||
* extKalman_t p; //定义一个卡尔曼滤波器结构体
|
||||
* float SersorData; //需要进行滤波的数据
|
||||
* KalmanCreate(&p,20,200); //初始化该滤波器的Q=20 R=200参数
|
||||
* while(1)
|
||||
* {
|
||||
* SersorData = sersor(); //获取数据
|
||||
* SersorData = KalmanFilter(&p,SersorData); //对数据进行滤波
|
||||
* }
|
||||
*/
|
||||
|
||||
#include "kalman.h"
|
||||
|
||||
/**
|
||||
* @name kalmanCreate
|
||||
* @brief 创建一个卡尔曼滤波器
|
||||
* @param p: 滤波器
|
||||
* T_Q:系统噪声协方差
|
||||
* T_R:测量噪声协方差
|
||||
*
|
||||
* @retval none
|
||||
* @attention R固定，Q越大，代表越信任侧量值，Q无穷代表只用测量值
|
||||
* 反之，Q越小代表越信任模型预测值，Q为零则是只用模型预测
|
||||
*/
|
||||
void KalmanCreate(extKalman_t* p, float T_Q, float T_R)
|
||||
{
|
||||
p->X_last = (float)0;
|
||||
p->P_last = 0;
|
||||
p->Q = T_Q;
|
||||
p->R = T_R;
|
||||
p->A = 1;
|
||||
p->B = 0;
|
||||
p->H = 1;
|
||||
p->X_mid = p->X_last;
|
||||
}
|
||||
|
||||
/**
|
||||
* @name KalmanFilter
|
||||
* @brief 卡尔曼滤波器
|
||||
* @param p: 滤波器
|
||||
* dat:待滤波数据
|
||||
* @retval 滤波后的数据
|
||||
* @attention Z(k)是系统输入,即测量值 X(k|k)是卡尔曼滤波后的值,即最终输出
|
||||
* A=1 B=0 H=1 I=1 W(K) V(k)是高斯白噪声,叠加在测量值上了,可以不用管
|
||||
* 以下是卡尔曼的5个核心公式
|
||||
* 一阶H'即为它本身,否则为转置矩阵
|
||||
*/
|
||||
|
||||
float KalmanFilter(extKalman_t* p, float dat)
|
||||
{
|
||||
p->X_mid = p->A * p->X_last; //百度对应公式(1) x(k|k-1) = A*X(k-1|k-1)+B*U(k)+W(K)
|
||||
p->P_mid = p->A * p->P_last + p->Q; //百度对应公式(2) p(k|k-1) = A*p(k-1|k-1)*A'+Q
|
||||
p->kg = p->P_mid / (p->P_mid + p->R); //百度对应公式(4) kg(k) = p(k|k-1)*H'/(H*p(k|k-1)*H'+R)
|
||||
p->X_now = p->X_mid + p->kg * (dat - p->X_mid); //百度对应公式(3) x(k|k) = X(k|k-1)+kg(k)*(Z(k)-H*X(k|k-1))
|
||||
p->P_now = (1 - p->kg) * p->P_mid; //百度对应公式(5) p(k|k) = (I-kg(k)*H)*P(k|k-1)
|
||||
p->P_last = p->P_now; //状态更新
|
||||
p->X_last = p->X_now;
|
||||
return p->X_now; //输出预测结果x(k|k)
|
||||
}
|
||||
@@ -0,0 +1,25 @@
|
||||
|
||||
#ifndef _KALMAN_H
|
||||
#define _KALMAN_H
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float X_last; //上一时刻的最优结果 X(k-|k-1)
|
||||
float X_mid; //当前时刻的预测结果 X(k|k-1)
|
||||
float X_now; //当前时刻的最优结果 X(k|k)
|
||||
float P_mid; //当前时刻预测结果的协方差 P(k|k-1)
|
||||
float P_now; //当前时刻最优结果的协方差 P(k|k)
|
||||
float P_last; //上一时刻最优结果的协方差 P(k-1|k-1)
|
||||
float kg; //kalman增益
|
||||
float A; //系统参数
|
||||
float B;
|
||||
float Q;
|
||||
float R;
|
||||
float H;
|
||||
} extKalman_t;
|
||||
|
||||
void KalmanCreate(extKalman_t* p, float T_Q, float T_R);
|
||||
float KalmanFilter(extKalman_t* p, float dat);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,354 @@
|
||||
#ifdef _WIN32
|
||||
#include <windows.h>
|
||||
#include <mmsystem.h>
|
||||
#pragma comment(lib, "winmm.lib")
|
||||
#pragma comment(lib, "user32.lib")
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <cmath>
|
||||
#else
|
||||
#include <alsa/asoundlib.h>
|
||||
#include <cmath>
|
||||
#endif
|
||||
|
||||
#include <iostream>
|
||||
#include <cstdint>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
#ifndef M_LN2
|
||||
#define M_LN2 0.693147180559945309417
|
||||
#endif
|
||||
|
||||
#ifdef _WIN32
|
||||
// Windows 回调结构
|
||||
struct PlaybackContext {
|
||||
volatile bool done;
|
||||
WAVEHDR* header;
|
||||
};
|
||||
|
||||
// Windows 播放完成回调函数
|
||||
void CALLBACK waveOutProc(HWAVEOUT hwo, UINT uMsg, DWORD_PTR dwInstance,
|
||||
DWORD_PTR dwParam1, DWORD_PTR dwParam2) {
|
||||
if (uMsg == WOM_DONE) {
|
||||
PlaybackContext* ctx = reinterpret_cast<PlaybackContext*>(dwInstance);
|
||||
ctx->done = true;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
extern "C" {
|
||||
|
||||
#ifdef _WIN32
|
||||
#define SND_EXPORT __declspec(dllexport)
|
||||
#else
|
||||
#define SND_EXPORT
|
||||
#endif
|
||||
|
||||
SND_EXPORT bool play_frequency(float frequency, int32_t duration_ms,
|
||||
double volume, int32_t channels,
|
||||
int32_t sample_rate) {
|
||||
// 参数验证
|
||||
if (frequency <= 0.0f || duration_ms <= 0 ||
|
||||
volume <= 0.0 || volume > 1.0 ||
|
||||
channels <= 0 || channels > 8 ||
|
||||
sample_rate <= 0) {
|
||||
std::cerr << "Invalid parameters" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 精确计算帧数：使用 round 而非截断
|
||||
// duration_ms / 1000.0 得到秒数，乘以采样率得到帧数
|
||||
int64_t num_frames = static_cast<int64_t>(
|
||||
std::round(static_cast<double>(sample_rate) * duration_ms / 1000.0)
|
||||
);
|
||||
|
||||
if (num_frames <= 0) {
|
||||
std::cerr << "Duration too short to generate any frames" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 计算实际播放时长（用于调试）
|
||||
double actual_duration_ms = static_cast<double>(num_frames) / sample_rate * 1000.0;
|
||||
std::cout << "Requested duration: " << duration_ms << " ms, "
|
||||
<< "Actual duration: " << actual_duration_ms << " ms, "
|
||||
<< "Frames: " << num_frames << std::endl;
|
||||
|
||||
// 计算总采样点数
|
||||
int32_t total_samples = static_cast<int32_t>(num_frames * channels);
|
||||
std::vector<int16_t> buffer(total_samples);
|
||||
|
||||
// 计算振幅（应用对数音量缩放以获得更自然的音量控制）
|
||||
int32_t amplitude = static_cast<int32_t>(32767.0 * volume);
|
||||
|
||||
// 生成正弦波数据
|
||||
for (int64_t i = 0; i < num_frames; i++) {
|
||||
// 使用 double 计算以提高精度
|
||||
double t = static_cast<double>(i) / sample_rate;
|
||||
double value = sin(2.0 * M_PI * frequency * t);
|
||||
|
||||
// 应用淡入淡出以避免咔嗒声
|
||||
int64_t fade_samples = static_cast<int64_t>(sample_rate * 0.005); // 5ms 淡入淡出
|
||||
double fade_factor = 1.0;
|
||||
|
||||
if (i < fade_samples) {
|
||||
// 淡入：从 0 到 1
|
||||
fade_factor = static_cast<double>(i) / fade_samples;
|
||||
} else if (i >= num_frames - fade_samples) {
|
||||
// 淡出：从 1 到 0
|
||||
fade_factor = static_cast<double>(num_frames - 1 - i) / fade_samples;
|
||||
}
|
||||
|
||||
int16_t sample = static_cast<int16_t>(
|
||||
amplitude * value * fade_factor
|
||||
);
|
||||
|
||||
// 填充所有声道
|
||||
for (int32_t ch = 0; ch < channels; ch++) {
|
||||
buffer[i * channels + ch] = sample;
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef _WIN32
|
||||
// ===== Windows 实现（带回调，精确计时） =====
|
||||
WAVEFORMATEX wfx;
|
||||
wfx.wFormatTag = WAVE_FORMAT_PCM;
|
||||
wfx.nChannels = channels;
|
||||
wfx.nSamplesPerSec = sample_rate;
|
||||
wfx.wBitsPerSample = 16;
|
||||
wfx.nBlockAlign = channels * 2; // 16-bit = 2 bytes per sample
|
||||
wfx.nAvgBytesPerSec = sample_rate * wfx.nBlockAlign;
|
||||
wfx.cbSize = 0;
|
||||
|
||||
PlaybackContext context;
|
||||
context.done = false;
|
||||
context.header = new WAVEHDR();
|
||||
|
||||
// 打开波形设备，使用回调函数
|
||||
HWAVEOUT hWaveOut;
|
||||
MMRESULT result = waveOutOpen(
|
||||
&hWaveOut,
|
||||
WAVE_MAPPER,
|
||||
&wfx,
|
||||
reinterpret_cast<DWORD_PTR>(waveOutProc), // 回调函数
|
||||
reinterpret_cast<DWORD_PTR>(&context), // 传递给回调的数据
|
||||
CALLBACK_FUNCTION
|
||||
);
|
||||
|
||||
if (result != MMSYSERR_NOERROR) {
|
||||
std::cerr << "Cannot open audio device. Error code: " << result << std::endl;
|
||||
delete context.header;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 准备 WAVEHDR
|
||||
WAVEHDR& header = *context.header;
|
||||
ZeroMemory(&header, sizeof(WAVEHDR));
|
||||
header.lpData = reinterpret_cast<char*>(buffer.data());
|
||||
header.dwBufferLength = static_cast<DWORD>(total_samples * sizeof(int16_t));
|
||||
header.dwFlags = 0;
|
||||
header.dwLoops = 0;
|
||||
|
||||
result = waveOutPrepareHeader(hWaveOut, &header, sizeof(WAVEHDR));
|
||||
if (result != MMSYSERR_NOERROR) {
|
||||
std::cerr << "Failed to prepare audio header. Error code: " << result << std::endl;
|
||||
waveOutClose(hWaveOut);
|
||||
delete context.header;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 写入音频数据
|
||||
result = waveOutWrite(hWaveOut, &header, sizeof(WAVEHDR));
|
||||
if (result != MMSYSERR_NOERROR) {
|
||||
std::cerr << "Failed to write audio data. Error code: " << result << std::endl;
|
||||
waveOutUnprepareHeader(hWaveOut, &header, sizeof(WAVEHDR));
|
||||
waveOutClose(hWaveOut);
|
||||
delete context.header;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 等待播放完成（使用事件等待而非轮询）
|
||||
auto start_time = timeGetTime();
|
||||
while (!context.done) {
|
||||
// 使用 MsgWaitForMultipleObjects 等待消息或超时
|
||||
// 这样可以在等待的同时处理 Windows 消息
|
||||
DWORD wait_result = MsgWaitForMultipleObjects(0, NULL, FALSE, 100, QS_ALLINPUT);
|
||||
|
||||
if (wait_result == WAIT_OBJECT_0) {
|
||||
// 处理 Windows 消息
|
||||
MSG msg;
|
||||
while (PeekMessage(&msg, NULL, 0, 0, PM_REMOVE)) {
|
||||
TranslateMessage(&msg);
|
||||
DispatchMessage(&msg);
|
||||
}
|
||||
}
|
||||
|
||||
// 超时检查（防止无限等待）
|
||||
DWORD elapsed = timeGetTime() - start_time;
|
||||
if (elapsed > static_cast<DWORD>(duration_ms + 1000)) {
|
||||
std::cerr << "Playback timeout" << std::endl;
|
||||
waveOutReset(hWaveOut);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
auto end_time = timeGetTime();
|
||||
DWORD actual_play_time = end_time - start_time;
|
||||
std::cout << "Actual playback time: " << actual_play_time << " ms" << std::endl;
|
||||
|
||||
// 清理
|
||||
waveOutUnprepareHeader(hWaveOut, &header, sizeof(WAVEHDR));
|
||||
waveOutClose(hWaveOut);
|
||||
delete context.header;
|
||||
|
||||
#else
|
||||
// ===== Linux ALSA 实现 =====
|
||||
snd_pcm_t *pcm_handle = nullptr;
|
||||
const char* device = "default";
|
||||
|
||||
int rc = snd_pcm_open(&pcm_handle, device, SND_PCM_STREAM_PLAYBACK, 0);
|
||||
if (rc < 0) {
|
||||
std::cerr << "Cannot open device '" << device << "': "
|
||||
<< snd_strerror(rc) << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 设置硬件参数
|
||||
snd_pcm_hw_params_t *hw_params;
|
||||
snd_pcm_hw_params_alloca(&hw_params);
|
||||
|
||||
rc = snd_pcm_hw_params_any(pcm_handle, hw_params);
|
||||
if (rc < 0) {
|
||||
std::cerr << "Cannot get hardware parameters: " << snd_strerror(rc) << std::endl;
|
||||
snd_pcm_close(pcm_handle);
|
||||
return false;
|
||||
}
|
||||
|
||||
rc = snd_pcm_hw_params_set_access(pcm_handle, hw_params,
|
||||
SND_PCM_ACCESS_RW_INTERLEAVED);
|
||||
if (rc < 0) {
|
||||
std::cerr << "Cannot set access type: " << snd_strerror(rc) << std::endl;
|
||||
snd_pcm_close(pcm_handle);
|
||||
return false;
|
||||
}
|
||||
|
||||
rc = snd_pcm_hw_params_set_format(pcm_handle, hw_params,
|
||||
SND_PCM_FORMAT_S16_LE);
|
||||
if (rc < 0) {
|
||||
std::cerr << "Cannot set sample format: " << snd_strerror(rc) << std::endl;
|
||||
snd_pcm_close(pcm_handle);
|
||||
return false;
|
||||
}
|
||||
|
||||
rc = snd_pcm_hw_params_set_channels(pcm_handle, hw_params, channels);
|
||||
if (rc < 0) {
|
||||
std::cerr << "Cannot set channel count: " << snd_strerror(rc) << std::endl;
|
||||
snd_pcm_close(pcm_handle);
|
||||
return false;
|
||||
}
|
||||
|
||||
unsigned int hw_sample_rate = static_cast<unsigned int>(sample_rate);
|
||||
rc = snd_pcm_hw_params_set_rate_near(pcm_handle, hw_params,
|
||||
&hw_sample_rate, 0);
|
||||
if (rc < 0) {
|
||||
std::cerr << "Cannot set sample rate: " << snd_strerror(rc) << std::endl;
|
||||
snd_pcm_close(pcm_handle);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (hw_sample_rate != static_cast<unsigned int>(sample_rate)) {
|
||||
std::cout << "Requested sample rate " << sample_rate
|
||||
<< " Hz not supported, using " << hw_sample_rate << " Hz" << std::endl;
|
||||
}
|
||||
|
||||
// 设置缓冲区大小和周期
|
||||
snd_pcm_uframes_t buffer_size;
|
||||
rc = snd_pcm_hw_params_get_buffer_size_max(hw_params, &buffer_size);
|
||||
if (buffer_size > static_cast<snd_pcm_uframes_t>(num_frames)) {
|
||||
snd_pcm_hw_params_set_buffer_size_near(pcm_handle, hw_params,
|
||||
&reinterpret_cast<snd_pcm_uframes_t&>(num_frames));
|
||||
}
|
||||
|
||||
rc = snd_pcm_hw_params(pcm_handle, hw_params);
|
||||
if (rc < 0) {
|
||||
std::cerr << "Cannot set hardware parameters: " << snd_strerror(rc) << std::endl;
|
||||
snd_pcm_close(pcm_handle);
|
||||
return false;
|
||||
}
|
||||
|
||||
// 设置软件参数
|
||||
snd_pcm_sw_params_t *sw_params;
|
||||
snd_pcm_sw_params_alloca(&sw_params);
|
||||
|
||||
rc = snd_pcm_sw_params_current(pcm_handle, sw_params);
|
||||
if (rc < 0) {
|
||||
std::cerr << "Cannot get software parameters: " << snd_strerror(rc) << std::endl;
|
||||
snd_pcm_close(pcm_handle);
|
||||
return false;
|
||||
}
|
||||
|
||||
rc = snd_pcm_sw_params_set_start_threshold(pcm_handle, sw_params,
|
||||
static_cast<snd_pcm_uframes_t>(num_frames));
|
||||
if (rc < 0) {
|
||||
std::cerr << "Cannot set start threshold: " << snd_strerror(rc) << std::endl;
|
||||
snd_pcm_close(pcm_handle);
|
||||
return false;
|
||||
}
|
||||
|
||||
rc = snd_pcm_sw_params(pcm_handle, sw_params);
|
||||
if (rc < 0) {
|
||||
std::cerr << "Cannot set software parameters: " << snd_strerror(rc) << std::endl;
|
||||
snd_pcm_close(pcm_handle);
|
||||
return false;
|
||||
}
|
||||
|
||||
// 播放音频（写入数据）
|
||||
snd_pcm_sframes_t frames_written = 0;
|
||||
snd_pcm_uframes_t frames_to_write = static_cast<snd_pcm_uframes_t>(num_frames);
|
||||
|
||||
// 循环写入直到所有数据都播放完毕
|
||||
while (frames_written < static_cast<snd_pcm_sframes_t>(num_frames)) {
|
||||
snd_pcm_uframes_t remaining = static_cast<snd_pcm_uframes_t>(num_frames - frames_written);
|
||||
|
||||
rc = snd_pcm_writei(pcm_handle,
|
||||
buffer.data() + (frames_written * channels),
|
||||
remaining);
|
||||
|
||||
if (rc == -EPIPE) {
|
||||
// 缓冲区下溢，需要恢复
|
||||
std::cerr << "Buffer underrun occurred" << std::endl;
|
||||
snd_pcm_prepare(pcm_handle);
|
||||
rc = snd_pcm_writei(pcm_handle,
|
||||
buffer.data() + (frames_written * channels),
|
||||
remaining);
|
||||
}
|
||||
|
||||
if (rc < 0) {
|
||||
std::cerr << "Error writing to PCM device: " << snd_strerror(rc) << std::endl;
|
||||
snd_pcm_close(pcm_handle);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (rc > 0) {
|
||||
frames_written += rc;
|
||||
}
|
||||
}
|
||||
|
||||
// 等待所有数据播放完成
|
||||
rc = snd_pcm_drain(pcm_handle);
|
||||
if (rc < 0) {
|
||||
std::cerr << "Error draining PCM device: " << snd_strerror(rc) << std::endl;
|
||||
snd_pcm_close(pcm_handle);
|
||||
return false;
|
||||
}
|
||||
|
||||
snd_pcm_close(pcm_handle);
|
||||
#endif
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // extern "C"
|
||||
Reference in new issue
Block a user