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