82 lines
1.8 KiB
C++
82 lines
1.8 KiB
C++
#include "imu.hpp"
|
|
#include <fcntl.h>
|
|
#include <unistd.h>
|
|
#include <cmath>
|
|
#include <cstring>
|
|
#include <termios.h>
|
|
|
|
YawTracker::YawTracker() : _fd(-1), _yaw(0), _unbounded_yaw(0), _gyro_z(0), _gyro_bias(0), _ready(false) {}
|
|
|
|
bool YawTracker::begin(const char* device, int baud)
|
|
{
|
|
_fd = open(device, O_RDWR | O_NOCTTY);
|
|
if (_fd < 0) return false;
|
|
|
|
termios opt;
|
|
tcgetattr(_fd, &opt);
|
|
cfsetispeed(&opt, B115200);
|
|
cfsetospeed(&opt, B115200);
|
|
opt.c_cflag |= (CLOCAL | CREAD);
|
|
opt.c_cflag &= ~PARENB;
|
|
opt.c_cflag &= ~CSTOPB;
|
|
opt.c_cflag &= ~CSIZE;
|
|
opt.c_cflag |= CS8;
|
|
tcsetattr(_fd, TCSANOW, &opt);
|
|
|
|
calibrate();
|
|
return true;
|
|
}
|
|
|
|
void YawTracker::calibrate(int samples)
|
|
{
|
|
float sum = 0;
|
|
for (int i = 0; i < samples; ++i)
|
|
{
|
|
int ret = _parseJY62();
|
|
if (ret > 0) sum += _gyro_z;
|
|
usleep(5000);
|
|
}
|
|
_gyro_bias = sum / samples;
|
|
_yaw = 0;
|
|
_unbounded_yaw = 0;
|
|
_ready = true;
|
|
}
|
|
|
|
void YawTracker::update(float dt)
|
|
{
|
|
if (_parseJY62() <= 0) return;
|
|
|
|
float gyro_filt = _gyro_z - _gyro_bias;
|
|
if (std::fabs(gyro_filt) < 0.03f) gyro_filt = 0; // 死区
|
|
|
|
_unbounded_yaw += gyro_filt * dt;
|
|
_yaw = _unbounded_yaw;
|
|
|
|
while (_yaw > 180.0f) _yaw -= 360.0f;
|
|
while (_yaw < -180.0f) _yaw += 360.0f;
|
|
}
|
|
|
|
int YawTracker::_parseJY62()
|
|
{
|
|
uint8 buf[11];
|
|
int total = 0;
|
|
while (total < 11)
|
|
{
|
|
int n = read(_fd, buf + total, 11 - total);
|
|
if (n <= 0) return -1;
|
|
total += n;
|
|
}
|
|
|
|
while (buf[0] != 0x55 && total > 10)
|
|
{
|
|
memmove(buf, buf + 1, 10);
|
|
int n = read(_fd, buf + 10, 1);
|
|
if (n <= 0) return -1;
|
|
}
|
|
if (buf[0] != 0x55) return -1;
|
|
|
|
int16 gz = (buf[5] << 8) | buf[4];
|
|
_gyro_z = gz * (2000.0f / 32768.0f); // ±2000°/s
|
|
return 1;
|
|
}
|