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); } }