bool MPU9250_DMP::read(float* acc, float* gyr, float* mag, int16_t* temp, ORIENTATION* ori) { if ( fifoAvailable() && dmpUpdateFifo() == INV_SUCCESS ) { if (acc) { acc[0] = calcAccel(ax); acc[1] = calcAccel(ay); acc[2] = calcAccel(az); } if (gyr) { gyr[0] = calcGyro(gx); gyr[1] = calcGyro(gy); gyr[2] = calcGyro(gz); } if (mag) { mag[0] = calcMag(mx); mag[1] = calcMag(my); mag[2] = calcMag(mz); } if ((_features & DMP_FEATURE_6X_LP_QUAT) && ori) { computeEulerAngles(); ori->pitch = pitch; ori->yaw = yaw; ori->roll = roll; } if (temp) { updateTemperature(); *temp = temperature; } return true; } return false; }
void dxJointAMotor::getInfo1( dxJoint::Info1 *info ) { info->m = 0; info->nub = 0; // compute the axes and angles, if in Euler mode if ( mode == dAMotorEuler ) { dVector3 ax[3]; computeGlobalAxes( ax ); computeEulerAngles( ax ); } // see if we're powered or at a joint limit for each axis for ( int i = 0; i < num; i++ ) { if ( limot[i].testRotationalLimit( angle[i] ) || limot[i].fmax > 0 ) { info->m++; } } }
void ModifyAddPart::param(const char *name) { const char *text; // cerr << "Paramname is " << name << endl; // parameter order: // ventPath // Vent // Vent_0:Name // Vent_0:Pos // Vent_0:Dir // Vent_1:Name // Vent_1:Pos // Vent_1:Dir // ... // Set Action // check user action if (strcmp(name, p_action->getName()) == 0) { currAction = p_action->getValue(); return; } // check vent directory name=ventPath if (strcmp(name, p_ventDir->getName()) == 0) { text = p_ventDir->getValue(); if (strcmp(text, ventFilePath) == NULL) // nothing changed return; if (ventFilePath) delete[] ventFilePath; ventFilePath = new char[strlen(text) + 1]; strcpy(ventFilePath, text); numVentDirs = 1; getVentDirs(); return; } int k; for (int i = 0; i < MAX_VENTS; i++) { // check vent_? domainfile if (strcmp(name, p_name[i]->getName()) == 0) { currVentFile[i] = p_name[i]->getValue(); //cerr << "currStructure" << ventdirs[currVentFile[i]] << endl; if (strcmp(ventdirs[currVentFile[i]], Nothing) == NULL) exist[i] = 0; else exist[i] = 1; //cerr << "Vent "<< i << " set to " << ventdirs[currVentFile[i]] << endl; //cerr << "Vent "<< i << " state " << exist[i] << endl; return; } // check vent_? position if (strcmp(name, p_pos[i]->getName()) == 0) { for (k = 0; k < 3; k++) { pos[i][k] = p_pos[i]->getValue(k); } return; } // the matrix can be set only be feedback from COVER // because it is deactivated in the Control Panel // if COVER sends the matrix // compute the euler angles if (strcmp(name, p_rot[i]->getName()) == 0) { for (k = 0; k < 9; k++) { coverRot[i][k] = p_rot[i]->getValue(k); } computeEulerAngles(i); return; } // if the euler angles are set through the Control Panel // compute the matrix if (strcmp(name, p_euler[i]->getName()) == 0) { for (k = 0; k < 3; k++) { euler[i][k] = p_euler[i]->getValue(k); } computeCoverRot(i); return; } } }