void MxCore::rotateLocal( double angle, const osg::Vec3d& axis )
{
const osg::Matrixd r = osg::Matrixd::rotate( angle * _rotateScale, axis );
_viewDir = _viewDir * r;
_viewUp = _viewUp * r;
orthonormalize();
}
void MxCore::setByMatrix( const osg::Matrixd& m )
{
_viewUp.set( m( 1, 0 ), m( 1, 1 ), m( 1, 2 ) );
_viewDir.set( -m( 2, 0 ), -m( 2, 1 ), -m( 2, 2 ) );
_position.set( m( 3, 0 ), m( 3, 1 ), m( 3, 2 ) );
orthonormalize();
}
void Camera::turnDown(double deltaAngle) {
deltaAngle = degreesToRadians(deltaAngle);
double ca = cos(deltaAngle);
double sa = sin(deltaAngle);
setForward(*forward * ca - *up * sa);
setUp(*up * ca + *forward * sa);
orthonormalize();
}
void Camera::turnLeft(double deltaAngle) {
deltaAngle = degreesToRadians(deltaAngle);
double ca = cos(deltaAngle);
double sa = sin(deltaAngle);
Vec3 right = forward->cross(*up);
setForward(*forward * ca - right * sa);
orthonormalize();
}
void MxCore::rotateOrbit( double angle, const osg::Vec3d& axis )
{
const osg::Matrixd r = osg::Matrixd::rotate( angle * _rotateScale, axis );
_position = ( _position - _orbitCenter ) * r + _orbitCenter;
_viewDir = _viewDir * r;
_viewUp = _viewUp * r;
orthonormalize();
}
void MxCore::lookAtOrbitCenter()
{
osg::Vec3d newDir = _orbitCenter - _position;
newDir.normalize();
const osg::Matrixd r = osg::Matrixd::rotate( _viewDir, newDir );
_viewDir = _viewDir * r;
_viewUp = _viewUp * r;
orthonormalize();
}
frame3f patch_frame(Patch* patch, int elementid, const vec2f& uv) {
auto p = patch->cubic[elementid];
frame3f frame;
frame.o = interpolate_bezier_bicubic(patch->pos, p, uv);
frame.x = interpolate_bezier_bicubic_derivativex(patch->pos, p, uv);
frame.y = interpolate_bezier_bicubic_derivativey(patch->pos, p, uv);
frame.z = normalize(cross(frame.x,frame.y));
frame = orthonormalize(frame);
return frame;
}
frame3f lineset_cylinder_frame(LineSet* lines, int elementid) {
auto l = lines->line[elementid];
frame3f f;
f.o = lines->pos[l.x];
f.z = normalize(lines->pos[l.y]-lines->pos[l.x]);
f.x = x3f;
f.y = y3f;
f = orthonormalize(f);
return f;
}
void MxCore::reset()
{
_viewUp = _initialUp;
_viewDir = _initialDir;
orthonormalize();
_position = _initialPosition;
_fovy = _initialFovy;
setOrtho( false );
}
frame3f trianglemesh_frame(TriangleMesh* mesh, int elementid, const vec2f& uv) {
auto f = mesh->triangle[elementid];
frame3f ff;
ff.x = normalize(mesh->pos[f.y]-mesh->pos[f.x]);
ff.y = normalize(mesh->pos[f.z]-mesh->pos[f.x]);
if(not mesh->norm.empty()) ff.z = normalize(interpolate_baricentric_triangle(mesh->norm, f, uv));
else ff.z = triangle_normal(mesh->pos[f.x], mesh->pos[f.y], mesh->pos[f.z]);
ff.o = interpolate_baricentric_triangle(mesh->pos, f, uv);
ff = orthonormalize(ff);
return ff;
}
void LLCoordFrame::rotate(const LLMatrix3 &rotation_matrix)
{
mXAxis.rotVec(rotation_matrix);
mYAxis.rotVec(rotation_matrix);
orthonormalize();
if( !isFinite() )
{
reset();
llwarns << "Non Finite in LLCoordFrame::rotate()" << llendl;
}
}
frame3f mesh_frame(Mesh* mesh, int elementid, const vec2f& uv) {
auto f = mesh_triangle_face(mesh,elementid);
frame3f ff;
ff.x = normalize(mesh->pos[f.y]-mesh->pos[f.x]);
ff.y = normalize(mesh->pos[f.z]-mesh->pos[f.x]);
if(not mesh->norm.empty()) ff.z = normalize(interpolate_baricentric_triangle(mesh->norm,f,uv));
else if(elementid < mesh->triangle.size()) ff.z = triangle_normal(mesh->pos[f.x], mesh->pos[f.y], mesh->pos[f.z]);
else { auto f = mesh->quad[(elementid-mesh->triangle.size())/2]; ff.z = quad_normal(mesh->pos[f.x], mesh->pos[f.y], mesh->pos[f.z], mesh->pos[f.w]); }
ff.o = interpolate_baricentric_triangle(mesh->pos,f,uv);
ff = orthonormalize(ff);
return ff;
}
void MxCore::level()
{
_viewUp = _initialUp;
// Check for vurrent view dir coincident with initial up vector. If so,
// we can't preserve the current view dir and need to set it to the
// initial view dir.
if( osg::absolute< double >( _initialUp * _viewDir ) > 0.99 )
_viewDir = _initialDir;
else
_viewDir = _viewUp ^ getCross();
orthonormalize();
}
/// Return the frame of a spline
/// @param spline The spline shape
/// @param elementid The index for the spline segment the frame is to be evaluated for
/// @param u The normalized value ([0,1]) used for evaluating the spline or spline segment
/// @return The spline frame
frame3f spline_frame(Spline* spline, int elementid, float u) {
auto frame = identity_frame3f;
auto pos = spline->pos;
auto cubic = spline->cubic[elementid];
// Use interpolation functions to get the center and x of the frame
// Arbitrarily choose directions for the other two axes
// Then orthonormalize the frame
frame.o = interpolate_bezier_cubic(pos, cubic, u);
frame.x = interpolate_bezier_cubic_derivative(pos, cubic, u);
frame.y = y3f;
frame.z = z3f;
frame = orthonormalize(frame);
return frame;
}
inline void testOrthonormalization() {
// r = result from HTM::YawPitchRollXYZ_2_Rt(0.1, 0.4, 0.8, 0, 0, 0, R, t);
double ra[9] = {0.91646,0.208401,0.341571,0.0919527,0.721115,-0.686686,-0.389418,0.660729,0.641709};
DMatrix R(3,3);
for (unsigned int r=0; r<3; ++r)
for (unsigned int c=0; c<3; ++c)
R(r,c) = ra[r*3+c];
DMatrix Rt, invR; DVector t;
Rt = ublas::trans(R);
invR = inv(R);
std::cout << "R = " << R << std::endl;
std::cout << "det(R) = " << determinant(R) << std::endl;
std::cout << "Rt = " << Rt << std::endl;
std::cout << "RtR = " << ublas::prod(Rt,R) << std::endl;
std::cout << "inv(R) = " << invR << std::endl;
std::cout << "inv(R)*R=" << ublas::prod(invR,R) << std::endl;
std::cout << std::endl << "altering R a bit" << std::endl;
R(0,0) += 0.001;
R(1,0) += 0.004;
R(2,0) -= 0.01;
Rt = ublas::trans(R);
invR = inv(R);
std::cout << "R = " << R << std::endl;
std::cout << "det(R) = " << determinant(R) << std::endl;
std::cout << "Rt = " << Rt << std::endl;
std::cout << "RtR = " << ublas::prod(Rt,R) << std::endl;
std::cout << "inv(R) = " << invR << std::endl;
std::cout << "inv(R)*R=" << ublas::prod(invR,R) << std::endl;
std::cout << std::endl << "fixing R" << std::endl;
R = orthonormalize(R);
Rt = ublas::trans(R);
invR = inv(R);
std::cout << "R = " << R << std::endl;
std::cout << "det(R) = " << determinant(R) << std::endl;
std::cout << "Rt = " << Rt << std::endl;
std::cout << "RtR = " << ublas::prod(Rt,R) << std::endl;
std::cout << "inv(R) = " << invR << std::endl;
std::cout << "inv(R)*R=" << ublas::prod(invR,R) << std::endl;
}
frame3f shape_sample_uniform(Shape* shape, const vec2f& uv) {
if(is<Sphere>(shape)) {
auto sphere = cast<Sphere>(shape);
// see: http://mathworld.wolfram.com/SpherePointPicking.html
float z = 1 - 2*uv.y;
float rxy = sqrt(1-z*z);
float phi = 2*pif*uv.x;
vec3f pl = vec3f(rxy*cos(phi),rxy*sin(phi),z);
frame3f f;
f.o = sphere->center + pl * sphere->radius;
f.z = normalize(pl);
f = orthonormalize(f);
return f;
}
else if(is<Quad>(shape)) {
auto quad = cast<Quad>(shape);
frame3f f = identity_frame3f;
f.o = (uv.x-0.5f)*quad->width*x3f + (uv.y-0.5f)*quad->height*y3f;
return f;
}
else { not_implemented_error(); return identity_frame3f; }
}
tf::Matrix3x3 JPCT_Math::rotateAxis(tf::Vector3 axis, tf::Matrix3x3& original, float angle) {
if(angle != lastRot) {
lastRot = angle;
lastSin = (float)sin((double)angle);
lastCos = (float)cos((double)angle);
}
float c = lastCos;
float s = lastSin;
float t = 1.0F - c;
//axis = axis.normalize(axis);
float x = axis.x();
float y = axis.y();
float z = axis.z();
tf::Matrix3x3 mat;
mat.setIdentity();
float sy = s * y;
float sx = s * x;
float sz = s * z;
float txy = t * x * y;
float txz = t * x * z;
float tyz = t * y * z;
mat[0][0] = t * x * x + c;
mat[1][0] = txy + sz;
mat[2][0] = txz - sy;
mat[0][1] = txy - sz;
mat[1][1] = t * y * y + c;
mat[2][1] = tyz + sx;
mat[0][2] = txz + sy;
mat[1][2] = tyz - sx;
mat[2][2] = t * z * z + c;
orthonormalize(mat);
original *= mat;
return original;
}
void MxCore::setOrientationByMatrix( const osg::Matrixd& m )
{
_viewUp.set( m( 1, 0 ), m( 1, 1 ), m( 1, 2 ) );
_viewDir.set( -m( 2, 0 ), -m( 2, 1 ), -m( 2, 2 ) );
orthonormalize();
}
//this starts with current state
bool CLyapWolfMethod::calculate()
{
//get the pointer to the parent task. It's needed for output
mpTask = dynamic_cast<CLyapTask*>(getObjectParent());
assert(mpTask);
//initialize LSODA
start();
C_FLOAT64 stepSize = getValue< C_FLOAT64 >("Orthonormalization Interval");
C_FLOAT64 transientTime = mpProblem->getTransientTime() + *mpContainerStateTime;
C_FLOAT64 endTime = *mpContainerStateTime + getValue< C_FLOAT64 >("Overall time");
C_FLOAT64 startTime = *mpContainerStateTime;
bool flagProceed = true;
C_FLOAT64 handlerFactor = 100.0 / (endTime - startTime);
//** do the transient **
C_FLOAT64 CompareTime = transientTime - 100.0 * fabs(transientTime) * std::numeric_limits< C_FLOAT64 >::epsilon();
if (*mpContainerStateTime < CompareTime)
{
do
{
step(transientTime - *mpContainerStateTime);
if (*mpContainerStateTime > CompareTime) break;
/* Here we will do conditional event processing */
/* Currently this is correct since no events are processed. */
//CCopasiMessage(CCopasiMessage::EXCEPTION, MCTrajectoryMethod + 12);
flagProceed &= mpTask->methodCallback((*mpContainerStateTime - startTime) * handlerFactor, true);
}
while (flagProceed);
//mpLyapProblem->getModel()->setState(*mpCurrentState);
//mpLyapProblem->getModel()->updateSimulatedValues();
}
else
{
}
//copy state to model and do output
mpContainer->updateSimulatedValues(mReducedModel);
mpTask->methodCallback((*mpContainerStateTime - startTime) * handlerFactor, false);
//********
orthonormalize();
if (mDoDivergence)
*(mVariables.array() + mVariables.size() - 1) = 0; //divergence
mLsodaStatus = 1; //the state has changed, we need to restart lsoda
size_t i;
C_FLOAT64 realStepSize;
do
{
realStepSize = step(stepSize);
orthonormalize();
mLsodaStatus = 1; //the state has changed, we need to restart lsoda
//process results of orthonormalisation
for (i = 0; i < mNumExp; ++i)
{
mpTask->mLocalExponents[i] = log(mNorms[i]);
mSumExponents[i] += mpTask->mLocalExponents[i];
mpTask->mLocalExponents[i] = mpTask->mLocalExponents[i] / realStepSize;
mpTask->mExponents[i] = mSumExponents[i] / (*mpContainerStateTime - transientTime);
}
//process result of divergence integration
if (mDoDivergence)
{
mSumDivergence += *(mVariables.array() + mVariables.size() - 1);
mpTask->mIntervalDivergence = *(mVariables.array() + mVariables.size() - 1) / realStepSize;
*(mVariables.array() + mVariables.size() - 1) = 0;
mpTask->mAverageDivergence = mSumDivergence / (*mpContainerStateTime - transientTime);
}
flagProceed &= mpTask->methodCallback((*mpContainerStateTime - startTime) * handlerFactor, false);
}
while ((*mpContainerStateTime < endTime) && flagProceed);
return flagProceed;
}
void CLyapWolfMethod::start(/*const CState * initialState*/)
{
/* Reset lsoda */
mLsodaStatus = 1;
mState = 1;
mJType = 2;
mErrorMsg.str("");
mLSODA.setOstream(mErrorMsg);
mReducedModel = true; /* *getValue("Integrate Reduced Model").pBOOL;*/
/* Release previous state and make the initialState the current */
mContainerState.initialize(mpContainer->getState(mReducedModel));
//mY = mpState->beginIndependent();
mpContainerStateTime = mContainerState.array() + mpContainer->getCountFixedEventTargets();
mSystemSize = mContainerState.size() - mpContainer->getCountFixedEventTargets() - 1;
mpYdot = mpContainer->getRate(mReducedModel).array() + mpContainer->getCountFixedEventTargets() + 1;
mNumExp = mpProblem->getExponentNumber();
mDoDivergence = mpProblem->divergenceRequested();
//calculate the number of variables for lsoda integration
if (mDoDivergence)
mData.dim = (C_INT)(mSystemSize * (1 + mNumExp) + 1);
else
mData.dim = (C_INT)(mSystemSize * (1 + mNumExp));
//initialize the vector on which lsoda will work
mVariables.resize(mData.dim);
//reserve space for exponents. The vectors in the task are resized by the task because they
//need to have a minimum size defined in the task
//pTask->mLocalExponents.resize(mNumExp);
mSumExponents.resize(mNumExp);
mNorms.resize(mNumExp);
//pTask->mExponents.resize(mNumExp);
memcpy(mVariables.array(), mpContainerStateTime + 1, mSystemSize * sizeof(C_FLOAT64));
//generate base vectors. Just define some arbitrary starting vectors that are not too specific and orthonormalize
// first fill the array with 0.1
C_FLOAT64 *dbl, *dblEnd = mVariables.array() + mData.dim;
for (dbl = mVariables.array() + mSystemSize; dbl != dblEnd; ++dbl)
*dbl = 0.01;
//now add 1.0
if (mNumExp > 0)
for (dbl = mVariables.array() + mSystemSize; dbl < dblEnd; dbl += (mSystemSize + 1))
* dbl = 1.0;
orthonormalize();
//reserve space for jacobian
mJacobian.resize(mSystemSize, mSystemSize);
size_t i, imax = mNumExp;
for (i = 0; i < imax; ++i)
{
mpTask->mLocalExponents[i] = 0;
mSumExponents[i] = 0;
mpTask->mExponents[i] = 0;
}
mpTask->mIntervalDivergence = 0;
mSumDivergence = 0;
mpTask->mAverageDivergence = 0;
/* Configure lsoda */
mRtol = getValue< C_FLOAT64 >("Relative Tolerance");
C_FLOAT64 * pTolerance = &getValue< C_FLOAT64 >("Absolute Tolerance");
CVector< C_FLOAT64 > tmpAtol = mpContainer->initializeAtolVector(*pTolerance, mReducedModel);
mAtol.resize(mData.dim);
for (i = 0; i < mSystemSize; ++i)
mAtol[i] = tmpAtol[i];
for (i = mSystemSize; (int)i < mData.dim; ++i)
mAtol[i] = 1e-20;
mDWork.resize(22 + mData.dim * std::max<C_INT>(16, mData.dim + 9));
mDWork[4] = mDWork[5] = mDWork[6] = mDWork[7] = mDWork[8] = mDWork[9] = 0.0;
mIWork.resize(20 + mData.dim);
mIWork[4] = mIWork[6] = mIWork[9] = 0;
mIWork[5] = getValue< unsigned C_INT32 >("Max Internal Steps");
mIWork[7] = 12;
mIWork[8] = 5;
return;
}
void update_ahrs(float dt, float dcm_out[3][3], float gyr_out[3])
{
static uint8_t i;
read_mpu(v_gyr, v_acc);
for (i=0; i<3; i++) {
gyr_out[i] = v_gyr[i];
}
/**
* Accelerometer
* Frame of reference: body
* Units: G (gravitational acceleration)
* Purpose: Measure the acceleration vector v_acc with components
* codirectional with the i, j, and k vectors. Note that the
* gravitational vector is the negative of the K vector.
*/
#ifdef ACC_WEIGHT
// Take weighted average.
#ifdef ACC_SELF_WEIGHT
for (i=0; i<3; i++) {
v_acc[i] = ACC_SELF_WEIGHT * v_acc[i] + (1-ACC_SELF_WEIGHT) * v_acc_last[i];
v_acc_last[i] = v_acc[i];
// Kalman filtering?
//v_acc_last[i] = acc.get(i);
//kalmanUpdate(v_acc[i], accVar, v_acc_last[i], ACC_UPDATE_SIG);
//kalmanPredict(v_acc[i], accVar, 0.0, ACC_PREDICT_SIG);
}
#endif // ACC_SELF_WEIGHT
acc_scale = v_norm(v_acc);
/**
* Reduce accelerometer weight if the magnitude of the measured
* acceleration is significantly greater than or less than 1 g.
*
* TODO: Magnitude of acceleration should be reported over telemetry so
* ACC_SCALE_WEIGHT can be more accurately determined.
*/
#ifdef ACC_SCALE_WEIGHT
acc_scale = (1.0 - MIN(1.0, ACC_SCALE_WEIGHT * ABS(acc_scale - 1.0)));
acc_weight = ACC_WEIGHT * acc_scale;
#else
acc_weight = ACC_WEIGHT;
// Ignore accelerometer if it measures anything 0.5g past gravity.
if (acc_scale > 1.5 || acc_scale < 0.5) {
acc_weight = 0;
}
#endif // ACC_SCALE_WEIGHT
/**
* Express K global unit vector in body frame as k_gb for use in drift
* correction (we need K to be described in the body frame because gravity
* is measured by the accelerometer in the body frame). Technically we
* could just create a transpose of dcm_gyro, but since we don't (yet) have
* a magnetometer, we don't need the first two rows of the transpose. This
* saves a few clock cycles.
*/
for (i=0; i<3; i++) {
k_gb[i] = dcm_gyro[i][2];
}
/**
* Calculate gyro drift correction rotation vector w_a, which will be used
* later to bring KB closer to the gravity vector (i.e., the negative of
* the K vector). Although we do not explicitly negate the gravity vector,
* the cross product below produces a rotation vector that we can later add
* to the angular displacement vector to correct for gyro drift in the
* X and Y axes. Note we negate w_a because our acceleration vector is
* actually the negative of our gravity vector.
*/
v_crossp(k_gb, v_acc, w_a);
v_scale(w_a, -1, w_a);
#endif // ACC_WEIGHT
/**
* Magnetometer
* Frame of reference: body
* Units: N/A
* Purpose: Measure the magnetic north vector v_mag with components
* codirectional with the body's i, j, and k vectors.
*/
#ifdef MAG_WEIGHT
// Express J global unit vectory in body frame as j_gb.
for (i=0; i<3; i++) {
j_gb[i] = dcm_gyro[i][1];
}
// Calculate yaw drift correction vector w_m.
v_crossp(j_gb, v_mag, w_m);
#endif // MAG_WEIGHT
/**
* Gyroscope
* Frame of reference: body
* Units: rad/s
* Purpose: Measure the rotation rate of the body about the body's i,
* j, and k axes.
* ========================================================================
* Scale v_gyr by elapsed time (in seconds) to get angle w*dt in radians,
* then compute weighted average with the accelerometer and magnetometer
* correction vectors to obtain final w*dt.
* TODO: This is still not exactly correct.
*/
for (i=0; i<3; i++) {
w_dt[i] = v_gyr[i] * dt;
#ifdef ACC_WEIGHT
w_dt[i] += acc_weight * w_a[i];
#endif // ACC_WEIGHT
#ifdef MAG_WEIGHT
w_dt[i] += MAG_WEIGHT * w_m[i];
#endif // MAG_WEIGHT
}
/**
* Direction Cosine Matrix
* Frame of reference: global
* Units: None (unit vectors)
* Purpose: Calculate the components of the body's i, j, and k unit
* vectors in the global frame of reference.
* ========================================================================
* Skew the rotation vector and sum appropriate components by combining the
* skew symmetric matrix with the identity matrix. The math can be
* summarized as follows:
*
* All of this is calculated in the body frame. If w is the angular
* velocity vector, let w_dt (w*dt) be the angular displacement vector of
* the DCM over a time interval dt. Let w_dt_i, w_dt_j, and w_dt_k be the
* components of w_dt codirectional with the i, j, and k unit vectors,
* respectively. Also, let dr be the linear displacement vector of the DCM
* and dr_i, dr_j, and dr_k once again be the i, j, and k components,
* respectively.
*
* In very small dt, certain vectors approach orthogonality, so we can
* assume that (draw this out for yourself!):
*
* dr_x = < 0, dw_k, -dw_j>,
* dr_y = <-dw_k, 0, dw_i>, and
* dr_z = < dw_j, -dw_i, 0>,
*
* which can be expressed as the rotation matrix:
*
* [ 0 dw_k -dw_j ]
* dr = [ -dw_k 0 dw_i ]
* [ dw_j -dw_i 0 ].
*
* This can then be multiplied by the current DCM and added to the current
* DCM to update the DCM. To minimize the number of calculations performed
* by the processor, however, we can combine the last two steps by
* combining dr with the identity matrix to produce:
*
* [ 1 dw_k -dw_j ]
* dr + I = [ -dw_k 1 dw_i ]
* [ dw_j -dw_i 1 ],
*
* which we multiply with the current DCM to produce the updated DCM
* directly.
*
* It may be helpful to read the Wikipedia pages on cross products and
* rotation representation.
*/
dcm_d[0][0] = 1;
dcm_d[0][1] = w_dt[2];
dcm_d[0][2] = -w_dt[1];
dcm_d[1][0] = -w_dt[2];
dcm_d[1][1] = 1;
dcm_d[1][2] = w_dt[0];
dcm_d[2][0] = w_dt[1];
dcm_d[2][1] = -w_dt[0];
dcm_d[2][2] = 1;
// Multiply the current DCM with the change in DCM and update.
m_product(dcm_d, dcm_gyro, dcm_gyro);
// Remove any distortions introduced by the small-angle approximations.
orthonormalize(dcm_gyro);
// Apply rotational offset (in case IMU is not installed orthogonally to
// the airframe).
dcm_offset[0][0] = 1;
dcm_offset[0][1] = 0;
dcm_offset[0][2] = -trim_angle[1];
dcm_offset[1][0] = 0;
dcm_offset[1][1] = 1;
dcm_offset[1][2] = trim_angle[0];
dcm_offset[2][0] = trim_angle[1];
dcm_offset[2][1] = -trim_angle[0];
dcm_offset[2][2] = 1;
m_product(dcm_offset, dcm_gyro, dcm_out);
}
void IMU::update() {
// ========================================================================
// Accelerometer
// Frame of reference: BODY
// Units: G (gravitational acceleration)
// Purpose: Measure the acceleration vector aVec with components
// codirectional with the i, j, and k vectors. Note that the
// gravitational vector is the negative of the K vector.
// ========================================================================
#ifdef ACC_WEIGHT
if (loopCount % ACC_READ_INTERVAL == 0) {
acc.poll();
for (int i=0; i<3; i++) {
aVec[i] = acc.get(i);
}
// Take weighted average.
#ifdef ACC_SELF_WEIGHT
for (int i=0; i<3; i++) {
aVec[i] = ACC_SELF_WEIGHT * aVec[i] + (1-ACC_SELF_WEIGHT) * aVecLast[i];
aVecLast[i] = aVec[i];
// Kalman filtering?
//aVecLast[i] = acc.get(i);
//kalmanUpdate(aVec[i], accVar, aVecLast[i], ACC_UPDATE_SIG);
//kalmanPredict(aVec[i], accVar, 0.0, ACC_PREDICT_SIG);
}
#endif // ACC_SELF_WEIGHT
accScale = vNorm(aVec);
// Reduce accelerometer weight if the magnitude of the measured
// acceleration is significantly greater than or less than 1 g.
//
// TODO: Magnitude of acceleration should be reported over telemetry so
// the "cutoff" value (the constant before the ABS() below) for
// disregaring acceleration input can be more accurately determined.
#ifdef ACC_SCALE_WEIGHT
// For some reason, 1 gravity has magnitude of 1.05.
accScale = (1.0 - MIN(1.0, ACC_SCALE_WEIGHT * ABS(accScale - 1.0)));
accWeight = ACC_WEIGHT * accScale;
#else
accWeight = ACC_WEIGHT;
// Ignore accelerometer if it measures anything 0.5g past gravity.
if (accScale > 1.5 || accScale < 0.5) {
accWeight = 0;
}
#endif // ACC_SCALE_WEIGHT
// Uncomment the loop below to get accelerometer readings in order to
// obtain trimAngle.
//if (loopCount % COMM_LOOP_INTERVAL == 0) {
// sp("A(");
// sp(aVec[0]*1000); sp(", ");
// sp(aVec[1]*1000); sp(", ");
// sp(aVec[2]*1000);
// sp(") ");
//}
// Express K global unit vector in BODY frame as kgb for use in drift
// correction (we need K to be described in the BODY frame because
// gravity is measured by the accelerometer in the BODY frame).
// Technically we could just create a transpose of gyroDCM, but since
// we don't (yet) have a magnetometer, we don't need the first two rows
// of the transpose. This saves a few clock cycles.
for (int i=0; i<3; i++) {
kgb[i] = gyroDCM[i][2];
}
// Calculate gyro drift correction rotation vector wA, which will be
// used later to bring KB closer to the gravity vector (i.e., the
// negative of the K vector). Although we do not explicitly negate the
// gravity vector, the cross product below produces a rotation vector
// that we can later add to the angular displacement vector to correct
// for gyro drift in the X and Y axes.
vCrossP(kgb, aVec, wA);
// Divide by ACC_READ_INTERVAL since the acceleration vector is not
// updated every loop.
for (int i=0; i<3; i++) {
wA[i] /= ACC_READ_INTERVAL;
}
}
#endif // ACC_WEIGHT
// ========================================================================
// Magnetometer
// Frame of reference: BODY
// Units: N/A
// Purpose: Measure the magnetic north vector mVec with components
// codirectional with the body's i, j, and k vectors.
// ========================================================================
#ifdef MAG_WEIGHT
mag.poll(); // ?
for (int i=0; i<3; i++) {
mVec[i] = mag.get(i);
}
//if (loopCount % COMM_LOOP_INTERVAL == 0) {
// sp("M(");
// sp(mVec[0]); sp(", ");
// sp(mVec[1]); sp(", ");
// sp(mVec[2]);
// spln(")");
//}
// Express J global unit vectory in BODY frame as jgb.
for (int i=0; i<3; i++) {
jgb[i] = gyroDCM[i][1];
}
// Calculate yaw drift correction vector wM.
vCrossP(jgb, mVec, wM);
#endif // MAG_WEIGHT
// ========================================================================
// Gyroscope
// Frame of reference: BODY
// Units: rad/s
// Purpose: Measure the rotation rate of the body about the body's i,
// j, and k axes.
// ========================================================================
gyro.poll();
for (int i=0; i<3; i++) {
gVec[i] = gyro.get(i);
}
//if (loopCount % COMM_LOOP_INTERVAL == 0) {
// sp("G(");
// sp(gVec[0]); sp(", ");
// sp(gVec[1]); sp(", ");
// sp(gVec[2]);
// spln(")");
//}
// Scale gVec by elapsed time (in seconds) to get angle w*dt in
// radians, then compute weighted average with the accelerometer and
// magnetometer correction vectors to obtain final w*dt.
for (int i=0; i<3; i++) {
float numerator = gVec[i] * MASTER_DT/1000000;
float denominator = 1.0;
#ifdef ACC_WEIGHT
if (loopCount % ACC_READ_INTERVAL == 0) {
numerator += accWeight * wA[i];
denominator += accWeight;
}
#endif // ACC_WEIGHT
#ifdef MAG_WEIGHT
numerator += MAG_WEIGHT * wM[i];
denominator += MAG_WEIGHT;
#endif // MAG_WEIGHT
wdt[i] = numerator / denominator;
}
// ========================================================================
// Direction Cosine Matrix
// Frame of reference: GLOBAL
// Units: None (unit vectors)
// Purpose: Calculate the components of the body's i, j, and k unit
// vectors in the global frame of reference.
// ========================================================================
// Skew the rotation vector and sum appropriate components by combining the
// skew symmetric matrix with the identity matrix. The math can be
// summarized as follows:
//
// All of this is calculated in the BODY frame. If w is the angular
// velocity vector, let wdt (w*dt) be the angular displacement vector of
// the DCM over a time interval dt. Let wdt_i, wdt_j, and wdt_k be the
// components of wdt codirectional with the i, j, and k unit vectors,
// respectively. Also, let dr be the linear displacement vector of the DCM
// and dr_i, dr_j, and dr_k once again be the i, j, and k components,
// respectively.
//
// In very small dt, certain vectors approach orthogonality, so we can
// assume that (draw this out for yourself!):
//
// dr_x = < 0, dw_k, -dw_j>,
// dr_y = <-dw_k, 0, dw_i>, and
// dr_z = < dw_j, -dw_i, 0>,
//
// which can be expressed as the rotation matrix:
//
// [ 0 dw_k -dw_j ]
// dr = [ -dw_k 0 dw_i ]
// [ dw_j -dw_i 0 ].
//
// This can then be multiplied by the current DCM and added to the current
// DCM to update the DCM. To minimize the number of calculations performed
// by the processor, however, we can combine the last two steps by
// combining dr with the identity matrix to produce:
//
// [ 1 dw_k -dw_j ]
// dr + I = [ -dw_k 1 dw_i ]
// [ dw_j -dw_i 1 ],
//
// which we multiply with the current DCM to produce the updated DCM
// directly.
//
// It may be helpful to read the Wikipedia pages on cross products and
// rotation representation.
// ========================================================================
dDCM[0][0] = 1;
dDCM[0][1] = wdt[2];
dDCM[0][2] = -wdt[1];
dDCM[1][0] = -wdt[2];
dDCM[1][1] = 1;
dDCM[1][2] = wdt[0];
dDCM[2][0] = wdt[1];
dDCM[2][1] = -wdt[0];
dDCM[2][2] = 1;
// Multiply the current DCM with the change in DCM and update.
mProduct(dDCM, gyroDCM, gyroDCM);
orthonormalize(gyroDCM);
#ifdef ACC_WEIGHT
trimDCM[0][0] = 1;
trimDCM[0][1] = 0;
trimDCM[0][2] = -trimAngle[1];
trimDCM[1][0] = 0;
trimDCM[1][1] = 1;
trimDCM[1][2] = trimAngle[0];
trimDCM[2][0] = trimAngle[1];
trimDCM[2][1] = -trimAngle[0];
trimDCM[2][2] = 1;
// Rotate gyroDCM with trimDCM.
mProduct(trimDCM, gyroDCM, bodyDCM);
//orthonormalize(bodyDCM); // TODO: This shouldn't be necessary.
#else
for (int i=0; i<3; i++) {
for (int j=0; j<3; j++) {
bodyDCM[i][j] = gyroDCM[i][j];
}
}
#endif // ACC_WEIGHT
}
