bool MPU9250Device::getQuaternion(Quaternion *quaternion, volatile bool *hasMoreMeasurements) {
if (!checkFIFO()) {
*hasMoreMeasurements = false;
return false;
}
long unsigned int sensor_data;
long quat[4];
short gyro[3], accel[3], sensors;
unsigned char more;
dmp_read_fifo(gyro, accel, quat, &sensor_data, &sensors, &more);
quaternion -> setValues((float)quat[0] / QUAT_SCALEFACTOR, (float)quat[1] / QUAT_SCALEFACTOR,
(float)quat[2] / QUAT_SCALEFACTOR, (float)quat[3] / QUAT_SCALEFACTOR);
quaternion -> normalize();
*hasMoreMeasurements = (more != 0);
return true;
}
void Boss::listener()
{
while(true)
{
if (manRunning) {
if ( (shared->latestSensorWritten - shared->latestSensorRead) > MAN_DEAD_THRESHOLD ) {
std::cout << "Boss::listener() killing man due to inactivity" << std::endl;
print_info();
killMan();
continue;
}
}
checkFIFO();
usleep(BOSS_MAIN_LOOP_US);
}
}
