Example #1
0
void SurfaceObj::computeRotationMatrix(Vec3f axis, float angle, Mat3x3f& rot)
{
	float x,y,z;
	x=axis[0];
	y=axis[1];
	z=axis[2];

	rot(0,0)=x*x+(y*y+z*z)*cos(angle);
	rot(1,1)=y*y+(x*x+z*z)*cos(angle);
	rot(2,2)=z*z+(x*x+y*y)*cos(angle);
	rot(0,1)=(1-cos(angle))*x*y+z*sin(angle);
	rot(1,0)=(1-cos(angle))*x*y-z*sin(angle);
	rot(0,2)=(1-cos(angle))*x*z-y*sin(angle);
	rot(2,0)=(1-cos(angle))*z*x+y*sin(angle);
	rot(1,2)=(1-cos(angle))*y*z+x*sin(angle);
	rot(2,1)=(1-cos(angle))*z*y-x*sin(angle);

	rot.transpose();
};
Example #2
0
    system::error_code AttitudeControlSimple::update(asio::yield_context yctx)
    {
        Mat3x3f R = m_ISAttitudeEstimated.m_Value.toRotationMatrix();
        
        #if 1
        Mat3x3f R_sp = m_ISAttitudeSetpoint.m_Value.toRotationMatrix();
        #else
        #if 0
        Mat3x3f R_sp = Quaternionf(AngleAxisf(M_PI_4, Vec3f::UnitX())).toRotationMatrix();
        #else
        static once_flag initSetpoint;
        call_once(initSetpoint, [this]() { m_ISAttitudeSetpoint.m_Value = m_ISAttitudeEstimated.m_Value; } );
        Mat3x3f R_sp = m_ISAttitudeSetpoint.m_Value.toRotationMatrix();
        #endif
        #endif
        
        float dT = std::min(std::max(m_ISDT.m_Value, 0.002f), 0.02f);
        
        Vec3f R_z(R(0, 2), R(1, 2), R(2, 2));
        Vec3f R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
        
        Vec3f e_R = R.transpose() * (R_z.cross(R_sp_z));
        
        float e_R_z_sin = e_R.norm();
        float e_R_z_cos = R_z.dot(R_sp_z);

        float yaw_w = R_sp(2, 2) * R_sp(2, 2);
        
        Mat3x3f R_rp;
        
        if(e_R_z_sin > 0.0f)
        {
            float e_R_z_angle = std::atan2(e_R_z_sin, e_R_z_cos);
            Vec3f e_R_z_axis = e_R / e_R_z_sin;
            
            e_R = e_R_z_angle * e_R_z_axis;
            
            Mat3x3f e_R_cp = Mat3x3f::Zero();
            e_R_cp(0, 1) = -e_R_z_axis(2);
            e_R_cp(0, 2) = e_R_z_axis(1);
            e_R_cp(1, 0) = e_R_z_axis(2);
            e_R_cp(1, 2) = -e_R_z_axis(0);
            e_R_cp(2, 0) = -e_R_z_axis(1);
            e_R_cp(2, 1) = e_R_z_axis(0);

            R_rp = R * (Mat3x3f::Identity() + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
        }
        else
        {
            R_rp = R;
        }
        
        Vec3f R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
        Vec3f R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
        e_R(2) = std::atan2(R_rp_x.cross(R_sp_x).dot(R_sp_z), R_rp_x.dot(R_sp_x)) * yaw_w;
        
        if(e_R_z_cos < 0.0f)
        {
            Quaternionf q(R.transpose() * R_sp);
            Vec3f e_R_d = q.vec();
            e_R_d.normalize();
            e_R_d *= 2.0f * std::atan2(e_R_d.norm(), q.w());
            
            float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
            e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
        }
        
        Vec3f const rotationRateSetpoint = m_AttitudeP.cwiseProduct(e_R);
        Vec3f e_RR = rotationRateSetpoint - m_ISRotationRateMeasured.m_Value;
        
        
        Vec3f rotationRateControl =
            m_RotationRateP.cwiseProduct(e_RR) +
            m_RotationRateD.cwiseProduct(m_RotationRateMeasuredPrev - m_ISRotationRateMeasured.m_Value) / dT +
            m_RotationRateIError;
        
        m_RotationRateMeasuredPrev = m_ISRotationRateMeasured.m_Value;
        m_RotationRateSetpointPrev = rotationRateSetpoint;
        
        m_OSRotationRateSetpoint.m_Value = rotationRateControl;
        
        return base::makeErrorCode(base::kENoError);
    }