void transform(){ auto lookat = getAttitude() * Glas::Vector3f(0, 0, 1) + getPosition(); auto virtical = getAttitude() * Glas::Vector3f(0, 1, 0); m_Camera.setLookAt(lookat.X, lookat.Y, lookat.Z); m_Camera.setPosition(getPosition().X, getPosition().Y, getPosition().Z); m_Camera.setTilt(virtical.X,virtical.Y, virtical.Z); m_Camera.transform(); }
inline osg::Vec3d BodyTransform::getForward() { osg::Vec3d attitude = getAttitude() * osg::Vec3d(1,0,0); attitude.z() = 0; attitude.normalize(); return attitude; }
void BodyTransform::rotateVertically(double da) { if (_pitch + da > _maxPitch) { da = _maxPitch - _pitch; } else if (_pitch + da < -_maxPitch) { da = -_maxPitch - _pitch; } _pitch += da; setAttitude(getAttitude() * osg::Quat(-da, getLeft())); }
void BodyTransform::rotateHorizontally(double da) { setAttitude(getAttitude() * osg::Quat(-da, getUp())); }
Glas::Vector3f DefaultCamera::getLookAt() const{ return getPosition() + getAttitude() * Glas::Vector3f(0,0,1); }
void MayhonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) { float recipNorm; float halfex, halfey, halfez; float qa, qb, qc; // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { // find inverse squareroot of accelerometer data recipNorm = invSqrt(ax * ax + ay * ay + az * az); //gain scheduling for measured acceleration if((recipNorm>0.0017755f)&&(recipNorm<0.00217f)) //only use accelerometer if normal is between 1.1 and 0.9G { currentT = millis(); if(currentT < 5000) //give AHRS time to converge to initial conditions { f.OK_TO_ARM = 0; f.ARMED = 0; if(currentT > convergedT) { LEDPIN_TOGGLE; convergedT = currentT +50; } twoKp = 40;//accelerate AHRS towards initial conditions while multicopter is held steady } else//normal operation { twoKp = twoKpDef; } } else { twoKp = twoKpDefMin; //reduce proportional gain to reduce effects of erroneous corrections in pitch and roll } //Normalise accelerometer reading ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // Estimated direction of gravity and vector perpendicular to magnetic flux halfvx = q1 * q3 - q0 * q2; halfvy = q0 * q1 + q2 * q3; halfvz = q0 * q0 - 0.5f + q3 * q3; // Error is sum of cross product between estimated and measured direction of gravity halfex = (ay * halfvz - az * halfvy); halfey = (az * halfvx - ax * halfvz); halfez = (ax * halfvy - ay * halfvx); // Compute and apply integral feedback if enabled if(twoKi > 0.0f) { integralFBx += twoKi * halfex * samplePeriod; // integral error scaled by Ki integralFBy += twoKi * halfey * samplePeriod; integralFBz += twoKi * halfez * samplePeriod; gx += integralFBx; // apply integral feedback gy += integralFBy; gz += integralFBz; } else { integralFBx = 0.0f; // prevent integral windup integralFBy = 0.0f; integralFBz = 0.0f; } // Apply proportional feedback gx += twoKp * halfex; gy += twoKp * halfey; gz += twoKp * halfez; } // Integrate rate of change of quaternion gx *= (0.5f * samplePeriod); // pre-multiply common factors gy *= (0.5f * samplePeriod); gz *= (0.5f * samplePeriod); qa = q0; qb = q1; qc = q2; q0 += (-qb * gx - qc * gy - q3 * gz); q1 += (qa * gx + qc * gz - q3 * gy); q2 += (qa * gy - qb * gz + q3 * gx); q3 += (qa * gz + qb * gy - qc * gx); // Normalise quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; //convert to tait bryan angles and store getAttitude(); }
void MayhonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { float recipNorm; float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; float hx, hy, bx, bz; float halfwx, halfwy, halfwz; float halfex, halfey, halfez; float qa, qb, qc; uint32_t currentT; //save time at current iteration currentT = millis(); // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { return; } // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { //compute inverse squareroot of accelerometer readings recipNorm = invSqrt(ax * ax + ay * ay + az * az); //gain scheduling for feedback based on external accelerations if((recipNorm>0.0017755f)&&(recipNorm<0.00217f)) //high gain if normal is between 1.1 and 0.9G { if(currentT < 5000) //give AHRS time to converge to initial conditions { f.OK_TO_ARM = 0; f.ARMED = 0; if(currentT > convergedT) { LEDPIN_TOGGLE; convergedT = currentT +50; } twoKp = 40;//accelerate AHRS towards initial conditions while multicopter is held steady } else if(currentT > accelT)//normal operation { twoKp = twoKpDef; twoKi = twoKiDef; } } else { accelT = currentT + 500; //reduce gains for 200ms after acceleration detected twoKp = twoKpDefMin; //reduce proportional gain to reduce effects of erroneous corrections in pitch and roll twoKi = 0; //reset integral to prevent windup and remove unwanted effects of large accel errors } // Normalise accelerometer measurement ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // Normalise magnetometer measurement recipNorm = invSqrt(mx * mx + my * my + mz * mz); mx *= recipNorm; my *= recipNorm; mz *= recipNorm; // Auxiliary variables to avoid repeated arithmetic q0q0 = q0 * q0; q0q1 = q0 * q1; q0q2 = q0 * q2; q0q3 = q0 * q3; q1q1 = q1 * q1; q1q2 = q1 * q2; q1q3 = q1 * q3; q2q2 = q2 * q2; q2q3 = q2 * q3; q3q3 = q3 * q3; // Reference direction of Earth's magnetic field, finding how much inclination should exist by translating to estimated earth frame hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); bx = sqrt(hx * hx + hy * hy); bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2)); // Estimated direction of gravity and magnetic field halfvx = q1q3 - q0q2; halfvy = q0q1 + q2q3; halfvz = q0q0 - 0.5f + q3q3; halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); //translate the reading back into sensor frame halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); // Error is weighted sum of cross product between estimated direction and measured direction of field vectors halfex = 2*(AccFactor*(ay * halfvz - az * halfvy) + MagFactor*(my * halfwz - mz * halfwy))/(AccFactor+MagFactor);//reduce cross coupling in pitch and roll halfey = 2*(AccFactor*(az * halfvx - ax * halfvz) + MagFactor*(mz * halfwx - mx * halfwz))/(AccFactor+MagFactor); halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);//more emphasis in compass yaw rotation // Compute and apply integral feedback if enabled if(twoKi > 0.0f) { integralFBx += twoKi * halfex * samplePeriod; // integral error scaled by Ki integralFBy += twoKi * halfey * samplePeriod; integralFBz += twoKi * halfez * samplePeriod; constrain(integralFBx,-0.008f,0.008f); //constrain integral error to 0.5 degs/sample to prevent integral windup constrain(integralFBy,-0.008f,0.008f); //(roughly error of 1 rad for 8/gain milliseconds to saturate = 800milliseconds at 0.01 ki) constrain(integralFBz,-0.008f,0.008f); gx += integralFBx; // apply integral feedback gy += integralFBy; gz += integralFBz; } else { integralFBx = 0.0f; // prevent integral windup integralFBy = 0.0f; integralFBz = 0.0f; } // Apply proportional feedback gx += halfex*twoKp; gy += halfey*twoKp; gz += halfez*twoKp; } // Integrate rate of change of quaternion gx *= (0.5f * samplePeriod); // pre-multiply common factors gy *= (0.5f * samplePeriod); gz *= (0.5f * samplePeriod); qa = q0; qb = q1; qc = q2; q0 += (-qb * gx - qc * gy - q3 * gz); q1 += (qa * gx + qc * gz - q3 * gy); q2 += (qa * gy - qb * gz + q3 * gx); q3 += (qa * gz + qb * gy - qc * gx); // Normalise quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; //convert to tait bryan angles and store getAttitude(); }