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