void Timeout::exitSerialTimeout(void){
if(serialTimeout!=NULL) serialTimeout->deleteTimer();
serialTimeout=NULL;
resetSerial();
//if(dbg) Serial.println("exit serial timeout");
}
void SerialInterface::readData() {
#ifdef __APPLE__
// This array sets the rate of trailing averaging for each channel.
// If the sensor rate is 100Hz, 0.001 will make the long term average a 10-second average
const float AVG_RATE[] = {0.01, 0.01, 0.01, 0.01, 0.01, 0.01};
char bufchar[1];
int initialSamples = totalSamples;
while (read(serial_fd, &bufchar, 1) > 0) {
//std::cout << bufchar[0];
serial_buffer[serial_buffer_pos] = bufchar[0];
serial_buffer_pos++;
// Have we reached end of a line of input?
if ((bufchar[0] == '\n') || (serial_buffer_pos >= MAX_BUFFER)) {
std::string serialLine(serial_buffer, serial_buffer_pos-1);
//std::cout << serialLine << "\n";
int spot;
//int channel = 0;
std::string val;
for (int i = 0; i < NUM_CHANNELS + 2; i++) {
spot = serialLine.find_first_of(" ", 0);
if (spot != std::string::npos) {
val = serialLine.substr(0,spot);
//std::cout << val << "\n";
if (i < NUM_CHANNELS) lastMeasured[i] = atoi(val.c_str());
else samplesAveraged = atoi(val.c_str());
} else LED = atoi(serialLine.c_str());
serialLine = serialLine.substr(spot+1, serialLine.length() - spot - 1);
}
for (int i = 0; i < NUM_CHANNELS; i++) {
if (totalSamples > SAMPLES_TO_DISCARD) {
trailingAverage[i] = (1.f - AVG_RATE[i])*trailingAverage[i] +
AVG_RATE[i]*(float)lastMeasured[i];
} else {
trailingAverage[i] = (float)lastMeasured[i];
}
}
totalSamples++;
serial_buffer_pos = 0;
}
}
if (initialSamples == totalSamples) {
timeval now;
gettimeofday(&now, NULL);
if (diffclock(&lastGoodRead, &now) > NO_READ_MAXIMUM_MSECS) {
std::cout << "No data coming over serial. Shutting down SerialInterface.\n";
resetSerial();
}
} else {
gettimeofday(&lastGoodRead, NULL);
}
#endif
}
// connect to the serial port
void SerialInterface::initializePort(char* portname) {
#ifndef _WIN32
_serialDescriptor = open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
qDebug("Opening SerialUSB %s: ", portname);
if (_serialDescriptor == -1) {
qDebug("Failed.\n");
return;
}
struct termios options;
tcgetattr(_serialDescriptor, &options);
options.c_cflag |= (CLOCAL | CREAD | CS8);
options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
tcsetattr(_serialDescriptor, TCSANOW, &options);
cfsetispeed(&options,B115200);
cfsetospeed(&options,B115200);
if (USING_INVENSENSE_MPU9150) {
// block on invensense reads until there is data to read
int currentFlags = fcntl(_serialDescriptor, F_GETFL);
fcntl(_serialDescriptor, F_SETFL, currentFlags & ~O_NONBLOCK);
// make sure there's nothing queued up to be read
tcflush(_serialDescriptor, TCIOFLUSH);
// this disables streaming so there's no garbage data on reads
if (write(_serialDescriptor, "SD\n", 3) != 3) {
qDebug("Failed.\n");
return;
}
char result[4];
if (read(_serialDescriptor, result, 4) != 4) {
qDebug("Failed.\n");
return;
}
tty_set_file_descriptor(_serialDescriptor);
mpu_init(0);
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
}
qDebug("Connected.\n");
resetSerial();
_active = true;
#endif
}
// connect to the serial port
void SerialInterface::initializePort(char* portname) {
#ifdef __APPLE__
_serialDescriptor = open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
printLog("Opening SerialUSB %s: ", portname);
if (_serialDescriptor == -1) {
printLog("Failed.\n");
return;
}
struct termios options;
tcgetattr(_serialDescriptor, &options);
options.c_cflag |= (CLOCAL | CREAD | CS8);
options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
tcsetattr(_serialDescriptor, TCSANOW, &options);
cfsetispeed(&options,B115200);
cfsetospeed(&options,B115200);
if (USING_INVENSENSE_MPU9150) {
// block on invensense reads until there is data to read
int currentFlags = fcntl(_serialDescriptor, F_GETFL);
fcntl(_serialDescriptor, F_SETFL, currentFlags & ~O_NONBLOCK);
// there are extra commands to send to the invensense when it fires up
// this takes it out of SLEEP
write(_serialDescriptor, "WR686B01\n", 9);
// delay after the wakeup
usleep(10000);
// this disables streaming so there's no garbage data on reads
write(_serialDescriptor, "SD\n", 3);
// delay after disabling streaming
usleep(10000);
// flush whatever was produced by the last two commands
tcflush(_serialDescriptor, TCIOFLUSH);
}
printLog("Connected.\n");
resetSerial();
_active = true;
#endif
}
// Init the serial port to the specified values
int SerialInterface::init(char* portname, int baud)
{
#ifdef __APPLE__
serial_fd = open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
printf("Attemping to open serial interface: %s\n", portname);
if (serial_fd == -1) return -1; // Failed to open port
struct termios options;
tcgetattr(serial_fd,&options);
switch(baud)
{
case 9600:
cfsetispeed(&options,B9600);
cfsetospeed(&options,B9600);
break;
case 19200:
cfsetispeed(&options,B19200);
cfsetospeed(&options,B19200);
break;
case 38400:
cfsetispeed(&options,B38400);
cfsetospeed(&options,B38400);
break;
case 115200:
cfsetispeed(&options,B115200);
cfsetospeed(&options,B115200);
break;
default:
cfsetispeed(&options,B9600);
cfsetospeed(&options,B9600);
break;
}
options.c_cflag |= (CLOCAL | CREAD);
options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8;
tcsetattr(serial_fd,TCSANOW,&options);
printf("Serial interface opened!\n");
resetSerial();
active = true;
#endif
return 0;
}
void SerialInterface::readData(float deltaTime) {
#ifndef _WIN32
int initialSamples = totalSamples;
if (USING_INVENSENSE_MPU9150) {
// ask the invensense for raw gyro data
short accelData[3];
if (mpu_get_accel_reg(accelData, 0)) {
close(_serialDescriptor);
qDebug("Disconnected SerialUSB.\n");
_active = false;
return; // disconnected
}
const float LSB_TO_METERS_PER_SECOND2 = 1.f / 16384.f * GRAVITY_EARTH;
// From MPU-9150 register map, with setting on
// highest resolution = +/- 2G
_lastAcceleration = glm::vec3(-accelData[2], -accelData[1], -accelData[0]) * LSB_TO_METERS_PER_SECOND2;
short gyroData[3];
mpu_get_gyro_reg(gyroData, 0);
// Convert the integer rates to floats
const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f; // From MPU-9150 register map, 2000 deg/sec.
glm::vec3 rotationRates;
rotationRates[0] = ((float) -gyroData[2]) * LSB_TO_DEGREES_PER_SECOND;
rotationRates[1] = ((float) -gyroData[1]) * LSB_TO_DEGREES_PER_SECOND;
rotationRates[2] = ((float) -gyroData[0]) * LSB_TO_DEGREES_PER_SECOND;
short compassData[3];
mpu_get_compass_reg(compassData, 0);
// Convert integer values to floats, update extents
_lastCompass = glm::vec3(compassData[2], -compassData[0], -compassData[1]);
// update and subtract the long term average
_averageRotationRates = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates +
1.f/(float)LONG_TERM_RATE_SAMPLES * rotationRates;
rotationRates -= _averageRotationRates;
// compute the angular acceleration
glm::vec3 angularAcceleration = (deltaTime < EPSILON) ? glm::vec3() : (rotationRates - _lastRotationRates) / deltaTime;
_lastRotationRates = rotationRates;
// Update raw rotation estimates
glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) *
glm::quat(glm::radians(deltaTime * _lastRotationRates));
// Update acceleration estimate: first, subtract gravity as rotated into current frame
_estimatedAcceleration = (totalSamples < GRAVITY_SAMPLES) ? glm::vec3() :
_lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
// update and subtract the long term average
_averageAcceleration = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration +
1.f/(float)LONG_TERM_RATE_SAMPLES * _estimatedAcceleration;
_estimatedAcceleration -= _averageAcceleration;
// Consider updating our angular velocity/acceleration to linear acceleration mapping
if (glm::length(_estimatedAcceleration) > EPSILON &&
(glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON)) {
// compute predicted linear acceleration, find error between actual and predicted
glm::vec3 predictedAcceleration = _angularVelocityToLinearAccel * _lastRotationRates +
_angularAccelToLinearAccel * angularAcceleration;
glm::vec3 error = _estimatedAcceleration - predictedAcceleration;
// the "error" is actually what we want: the linear acceleration minus rotational influences
_estimatedAcceleration = error;
// adjust according to error in each dimension, in proportion to input magnitudes
for (int i = 0; i < 3; i++) {
if (fabsf(error[i]) < EPSILON) {
continue;
}
const float LEARNING_RATE = 0.001f;
float rateSum = fabsf(_lastRotationRates.x) + fabsf(_lastRotationRates.y) + fabsf(_lastRotationRates.z);
if (rateSum > EPSILON) {
for (int j = 0; j < 3; j++) {
float proportion = LEARNING_RATE * fabsf(_lastRotationRates[j]) / rateSum;
if (proportion > EPSILON) {
_angularVelocityToLinearAccel[j][i] += error[i] * proportion / _lastRotationRates[j];
}
}
}
float accelSum = fabsf(angularAcceleration.x) + fabsf(angularAcceleration.y) + fabsf(angularAcceleration.z);
if (accelSum > EPSILON) {
for (int j = 0; j < 3; j++) {
float proportion = LEARNING_RATE * fabsf(angularAcceleration[j]) / accelSum;
if (proportion > EPSILON) {
_angularAccelToLinearAccel[j][i] += error[i] * proportion / angularAcceleration[j];
}
}
}
}
}
// rotate estimated acceleration into global rotation frame
_estimatedAcceleration = estimatedRotation * _estimatedAcceleration;
// Update estimated position and velocity
float const DECAY_VELOCITY = 0.975f;
float const DECAY_POSITION = 0.975f;
_estimatedVelocity += deltaTime * _estimatedAcceleration;
_estimatedPosition += deltaTime * _estimatedVelocity;
_estimatedVelocity *= DECAY_VELOCITY;
// Attempt to fuse gyro position with webcam position
Webcam* webcam = Application::getInstance()->getWebcam();
if (webcam->isActive()) {
const float WEBCAM_POSITION_FUSION = 0.5f;
_estimatedPosition = glm::mix(_estimatedPosition, webcam->getEstimatedPosition(), WEBCAM_POSITION_FUSION);
} else {
_estimatedPosition *= DECAY_POSITION;
}
// Accumulate a set of initial baseline readings for setting gravity
if (totalSamples == 0) {
_gravity = _lastAcceleration;
}
else {
if (totalSamples < GRAVITY_SAMPLES) {
_gravity = glm::mix(_gravity, _lastAcceleration, 1.0f / GRAVITY_SAMPLES);
// North samples start later, because the initial compass readings are screwy
int northSample = totalSamples - (GRAVITY_SAMPLES - NORTH_SAMPLES);
if (northSample == 0) {
_north = _lastCompass;
} else if (northSample > 0) {
_north = glm::mix(_north, _lastCompass, 1.0f / NORTH_SAMPLES);
}
} else {
// Use gravity reading to do sensor fusion on the pitch and roll estimation
estimatedRotation = safeMix(estimatedRotation,
rotationBetween(estimatedRotation * _lastAcceleration, _gravity) * estimatedRotation,
1.0f / ACCELERATION_SENSOR_FUSION_SAMPLES);
// Update the compass extents
_compassMinima = glm::min(_compassMinima, _lastCompass);
_compassMaxima = glm::max(_compassMaxima, _lastCompass);
// Same deal with the compass heading
estimatedRotation = safeMix(estimatedRotation,
rotationBetween(estimatedRotation * recenterCompass(_lastCompass),
recenterCompass(_north)) * estimatedRotation,
1.0f / COMPASS_SENSOR_FUSION_SAMPLES);
}
}
_estimatedRotation = safeEulerAngles(estimatedRotation);
totalSamples++;
}
if (initialSamples == totalSamples) {
timeval now;
gettimeofday(&now, NULL);
if (diffclock(&lastGoodRead, &now) > NO_READ_MAXIMUM_MSECS) {
qDebug("No data - Shutting down SerialInterface.\n");
resetSerial();
}
} else {
gettimeofday(&lastGoodRead, NULL);
}
#endif
}
void SerialInterface::readData(float deltaTime) {
#ifdef __APPLE__
int initialSamples = totalSamples;
if (USING_INVENSENSE_MPU9150) {
unsigned char sensorBuffer[36];
// ask the invensense for raw gyro data
write(_serialDescriptor, "RD683B0E\n", 9);
read(_serialDescriptor, sensorBuffer, 36);
int accelXRate, accelYRate, accelZRate;
convertHexToInt(sensorBuffer + 6, accelZRate);
convertHexToInt(sensorBuffer + 10, accelYRate);
convertHexToInt(sensorBuffer + 14, accelXRate);
const float LSB_TO_METERS_PER_SECOND2 = 1.f / 16384.f * GRAVITY_EARTH;
// From MPU-9150 register map, with setting on
// highest resolution = +/- 2G
_lastAcceleration = glm::vec3(-accelXRate, -accelYRate, -accelZRate) * LSB_TO_METERS_PER_SECOND2;
int rollRate, yawRate, pitchRate;
convertHexToInt(sensorBuffer + 22, rollRate);
convertHexToInt(sensorBuffer + 26, yawRate);
convertHexToInt(sensorBuffer + 30, pitchRate);
// Convert the integer rates to floats
const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f; // From MPU-9150 register map, 2000 deg/sec.
glm::vec3 rotationRates;
rotationRates[0] = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND;
rotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
rotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND;
// update and subtract the long term average
_averageRotationRates = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates +
1.f/(float)LONG_TERM_RATE_SAMPLES * rotationRates;
rotationRates -= _averageRotationRates;
// compute the angular acceleration
glm::vec3 angularAcceleration = (deltaTime < EPSILON) ? glm::vec3() : (rotationRates - _lastRotationRates) / deltaTime;
_lastRotationRates = rotationRates;
// Update raw rotation estimates
glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) *
glm::quat(glm::radians(deltaTime * _lastRotationRates));
// Update acceleration estimate: first, subtract gravity as rotated into current frame
_estimatedAcceleration = (totalSamples < GRAVITY_SAMPLES) ? glm::vec3() :
_lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
// update and subtract the long term average
_averageAcceleration = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration +
1.f/(float)LONG_TERM_RATE_SAMPLES * _estimatedAcceleration;
_estimatedAcceleration -= _averageAcceleration;
// Consider updating our angular velocity/acceleration to linear acceleration mapping
if (glm::length(_estimatedAcceleration) > EPSILON &&
(glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON)) {
// compute predicted linear acceleration, find error between actual and predicted
glm::vec3 predictedAcceleration = _angularVelocityToLinearAccel * _lastRotationRates +
_angularAccelToLinearAccel * angularAcceleration;
glm::vec3 error = _estimatedAcceleration - predictedAcceleration;
// the "error" is actually what we want: the linear acceleration minus rotational influences
_estimatedAcceleration = error;
// adjust according to error in each dimension, in proportion to input magnitudes
for (int i = 0; i < 3; i++) {
if (fabsf(error[i]) < EPSILON) {
continue;
}
const float LEARNING_RATE = 0.001f;
float rateSum = fabsf(_lastRotationRates.x) + fabsf(_lastRotationRates.y) + fabsf(_lastRotationRates.z);
if (rateSum > EPSILON) {
for (int j = 0; j < 3; j++) {
float proportion = LEARNING_RATE * fabsf(_lastRotationRates[j]) / rateSum;
if (proportion > EPSILON) {
_angularVelocityToLinearAccel[j][i] += error[i] * proportion / _lastRotationRates[j];
}
}
}
float accelSum = fabsf(angularAcceleration.x) + fabsf(angularAcceleration.y) + fabsf(angularAcceleration.z);
if (accelSum > EPSILON) {
for (int j = 0; j < 3; j++) {
float proportion = LEARNING_RATE * fabsf(angularAcceleration[j]) / accelSum;
if (proportion > EPSILON) {
_angularAccelToLinearAccel[j][i] += error[i] * proportion / angularAcceleration[j];
}
}
}
}
}
// rotate estimated acceleration into global rotation frame
_estimatedAcceleration = estimatedRotation * _estimatedAcceleration;
// Update estimated position and velocity
float const DECAY_VELOCITY = 0.975f;
float const DECAY_POSITION = 0.975f;
_estimatedVelocity += deltaTime * _estimatedAcceleration;
_estimatedPosition += deltaTime * _estimatedVelocity;
_estimatedVelocity *= DECAY_VELOCITY;
// Attempt to fuse gyro position with webcam position
Webcam* webcam = Application::getInstance()->getWebcam();
if (webcam->isActive()) {
const float WEBCAM_POSITION_FUSION = 0.5f;
_estimatedPosition = glm::mix(_estimatedPosition, webcam->getEstimatedPosition(), WEBCAM_POSITION_FUSION);
} else {
_estimatedPosition *= DECAY_POSITION;
}
// Accumulate a set of initial baseline readings for setting gravity
if (totalSamples == 0) {
_gravity = _lastAcceleration;
}
else {
if (totalSamples < GRAVITY_SAMPLES) {
_gravity = (1.f - 1.f/(float)GRAVITY_SAMPLES) * _gravity +
1.f/(float)GRAVITY_SAMPLES * _lastAcceleration;
} else {
// Use gravity reading to do sensor fusion on the pitch and roll estimation
estimatedRotation = safeMix(estimatedRotation,
rotationBetween(estimatedRotation * _lastAcceleration, _gravity) * estimatedRotation,
1.0f / SENSOR_FUSION_SAMPLES);
// Without a compass heading, always decay estimated Yaw slightly
const float YAW_DECAY = 0.999f;
glm::vec3 forward = estimatedRotation * glm::vec3(0.0f, 0.0f, -1.0f);
estimatedRotation = safeMix(glm::angleAxis(glm::degrees(atan2f(forward.x, -forward.z)),
glm::vec3(0.0f, 1.0f, 0.0f)) * estimatedRotation, estimatedRotation, YAW_DECAY);
}
}
_estimatedRotation = safeEulerAngles(estimatedRotation);
totalSamples++;
}
if (initialSamples == totalSamples) {
timeval now;
gettimeofday(&now, NULL);
if (diffclock(&lastGoodRead, &now) > NO_READ_MAXIMUM_MSECS) {
printLog("No data - Shutting down SerialInterface.\n");
resetSerial();
}
} else {
gettimeofday(&lastGoodRead, NULL);
}
#endif
}
