Task.hpp

Go to the documentation of this file.

/* Generated from orogen/lib/orogen/templates/tasks/Task.hpp */

#ifndef STIM300_TASK_TASK_HPP
#define STIM300_TASK_TASK_HPP

#include "imu_stim300/TaskBase.hpp"

#include <imu_stim300/Stim300Base.hpp>
#include <imu_stim300/Stim300RevB.hpp>
#include <imu_stim300/Stim300RevD.hpp>

#include <quater_ikf/Ikf.hpp> 
#include <aggregator/TimestampEstimator.hpp>
#include <rtt/extras/FileDescriptorActivity.hpp>

namespace imu_stim300 {

    static const int Re = 6378137; 
    static const int Rp = 6378137; 
    static const double ECC = 0.0818191908426; 
    static const double GRAVITY = 9.79766542; 
    static const double GWGS0 = 9.7803267714; 
    static const double GWGS1 = 0.00193185138639; 
    static const double EARTHW = 7.292115e-05; 
     enum CONST {
            IKFSTATEVECTORSIZE = filter::Ikf<double, true, true>::IKFSTATEVECTORSIZE
    };

    static const double GRAVITY_MARGIN = 0.3; 
    class Task : public TaskBase
    {
        friend class TaskBase;

    protected:

        /******************************/
        /*** Control Flow Variables ***/
        /******************************/

            bool initAttitude;

        unsigned int initial_alignment_idx;

        /**************************/
        /*** Property Variables ***/
        /**************************/

        FilterConfiguration config;

        InertialNoiseParameters accnoise;

        InertialNoiseParameters gyronoise;

        InertialNoiseParameters incnoise;

        AdaptiveAttitudeConfig adaptiveconfigAcc;
        AdaptiveAttitudeConfig adaptiveconfigInc;

        LocationConfiguration location;

        /**************************/
        /*** Internal Variables ***/
        /**************************/

        base::Time prev_ts;

        int correction_numbers, correction_idx;

        double sampling_frequency;

        imu_stim300::Stim300Base *imu_stim300_driver;
        aggregator::TimestampEstimator* timestamp_estimator;

        Eigen::Vector3d correctionAcc, correctionInc;

        Eigen::Matrix <double, 3, Eigen::Dynamic> initial_alignment_gyro;
        Eigen::Matrix <double, 3, Eigen::Dynamic> initial_alignment_acc;

        filter::Ikf<double, true, true> myfilter; 
        Eigen::Quaterniond deltaquat, deltahead, attitude;

        Eigen::Matrix4d oldomega;

        /***************************/
        /***************************/

        base::samples::RigidBodyState orientation_out; 
    public:
         Task(std::string const& name = "imu_stim300::Task");
        
        Task(std::string const& name, RTT::ExecutionEngine* engine);

        ~Task();

        bool configureHook();

        bool startHook();

        void updateHook();

        void errorHook();

        void stopHook();

        void cleanupHook();


        Eigen::Quaternion<double> deltaHeading(const Eigen::Vector3d &angvelo, Eigen::Matrix4d &oldomega, const double delta_t);


        void outputPortSamples(imu_stim300::Stim300Base *driver, filter::Ikf<double, true, true> &myfilter, base::samples::IMUSensors &imusamples);

        static double GravityModel(double latitude, double altitude)
        {
            double g; 
            g = GWGS0*((1+GWGS1*pow(sin(latitude),2))/sqrt(1-pow(ECC,2)*pow(sin(latitude),2)));

            g = g*pow(Re/(Re+altitude), 2);

                #ifdef DEBUG_PRINTS
            std::cout<<"[STIM300_CLASS] Theoretical gravity for this location (WGS-84 ellipsoid model): "<< g<<" [m/s^2]\n";
                #endif

            return g;
        };

        static void SubtractEarthRotation(Eigen::Vector3d &u, const Eigen::Quaterniond &q, const double latitude)
        {
            Eigen::Vector3d v (EARTHW*cos(latitude), 0, EARTHW*sin(latitude)); 
            v = q * v;

            #ifdef DEBUG_PRINTS
            std::cout<<"[STIM300_CLASS] Earth Rotation:"<<v<<"\n";
            #endif

            u  = u - v;

            return;
        };

        static Eigen::Quaternion<double> deltaQuaternion(const Eigen::Vector3d &angvelo, const Eigen::Matrix4d &oldomega4, const Eigen::Matrix4d &omega4, const double dt)
        {
            Eigen::Vector4d quat;
            quat<< 1.00, 0.00, 0.00, 0.00; 
            quat = (Eigen::Matrix<double,4,4>::Identity() +(0.75 * omega4 *dt)-(0.25 * oldomega4 * dt) -
            ((1.0/6.0) * angvelo.squaredNorm() * pow(dt,2) *  Eigen::Matrix<double, 4, 4>::Identity()) -
            ((1.0/24.0) * omega4 * oldomega4 * pow(dt,2)) - ((1.0/48.0) * angvelo.squaredNorm() * omega4 * pow(dt,3))) * quat;

            Eigen::Quaternion<double> deltaq;
            deltaq.w() = quat(0);
            deltaq.x() = quat(1);
            deltaq.y() = quat(2);
            deltaq.z() = quat(3);
            deltaq.normalize();

            return deltaq;
        };

    public:
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    };
}

#endif