source: flair-src/tags/latest/lib/FlairIpc/src/unexported/ipc_base.h

/**
 * \file ipc_base.h
 * \brief definition de structures
 * \author {
 *                      Sébastien Ambroziak
 *                      Paul Sauvage
 *                      }
 * \version 0.1
 * \date 5 novembre 2020
 *
 * Definitions des structures pour la message queue
 *
 */


#ifndef IPC_BASE
#define IPC_BASE

#include <string>
#include <iostream>

#define ROS2 1
#define FLAIR 2
#define ARDUPILOT 3

#include <ctime>

//#define MAX_TOPIC_SIZE 25
//#define MAX_CHAR_ARRAY_SIZE 255

// structures for message queue
namespace ipc{
namespace type{

struct vector2D {
        double x;
        double y;
};

struct vector2D_32 {
        float x;
        float y;
        float z;
};

inline std::ostream &operator<<(std::ostream &out, vector2D vecteur) {
    out << "\tx : " << vecteur.x << std::endl;
        out << "\ty : " << vecteur.y << std::endl;
    return out;
}

struct vector3D {
        double x;
        double y;
        double z;
};

struct vector3D_32 {
        float x;
        float y;
        float z;
};

inline std::ostream &operator<<(std::ostream &out, vector3D vecteur) {
    out << "\tx : " << vecteur.x << std::endl;
        out << "\ty : " << vecteur.y << std::endl;
        out << "\tz : " << vecteur.z << std::endl;
    return out;
}

inline std::ostream &operator<<(std::ostream &out, vector3D_32 vecteur) {
    out << "\tx : " << vecteur.x << std::endl;
        out << "\ty : " << vecteur.y << std::endl;
        out << "\tz : " << vecteur.z << std::endl;
    return out;
}

struct acceleration {
        vector3D linear;
        vector3D angular;
};

struct vector4D {
        double x;
        double y;
        double z;
        double w;
};

struct vector4D_32 {
        float x;
        float y;
        float z;
        float w;
};

inline std::ostream &operator<<(std::ostream &out, vector4D vecteur) {
    out << "\tx : " << vecteur.x << std::endl;
        out << "\ty : " << vecteur.y << std::endl;
        out << "\tz : " << vecteur.z << std::endl;
        out << "\tw : " << vecteur.w << std::endl;
    return out;
}

inline std::ostream &operator<<(std::ostream &out, vector4D_32 vecteur) {
    out << "\tx : " << vecteur.x << std::endl;
        out << "\ty : " << vecteur.y << std::endl;
        out << "\tz : " << vecteur.z << std::endl;
        out << "\tw : " << vecteur.w << std::endl;
    return out;
}

struct matrix {
        int width;
        int height;
        float data[5][5];
};

inline std::ostream &operator<<(std::ostream &out, matrix M) {
    
        for(int j = 0;j < M.height;j++){
                out<<"\t| "<<M.data[0][j];
                for(int i = 1;i < M.width;i++){
                        out << "\t " << M.data[i][j];
                }
                out << " |" << std::endl;
        }
    return out;
}

struct imu{
        vector4D orientation;
        double orientation_covariance[9];

        vector3D angular_velocity;
        double angular_velocity_covariance[9];

        vector3D linear_acceleration;
        double linear_acceleration_covariance[9];
};

inline std::ostream &operator<<(std::ostream &out, imu msg) {
        out << "orientation : \n" << msg.orientation << std::endl;
        out << "orientation covarience : " ;
        for (int i=0;i<9;i++){out << msg.orientation_covariance[i] << ' ';} 
        out << std::endl;

        out << "angular_velocity : \n" << msg.angular_velocity << std::endl;
        out << "angular_velocity_covariance : " ;
        for (int i=0;i<9;i++){out << msg.orientation_covariance[i] << ' ';} 
        out << std::endl;

        out << "linear_acceleration : \n" << msg.linear_acceleration << std::endl;
        out << "linear_acceleration_covariance : " ;
        for (int i=0;i<9;i++){out << msg.linear_acceleration_covariance[i] << ' ';} 
        out << std::endl;

    return out;
}

struct ahrs{
        vector4D_32 quaternion;
        vector3D_32 angular_rate;
};

inline std::ostream &operator<<(std::ostream &out, ahrs msg) {
        out << "quaternion : \n" << msg.quaternion << std::endl;
        out << "angular_rate : \n" << msg.angular_rate << std::endl;
    return out;
}


/**
 * \struct ipc_message
 * \brief Structure classique de message ipc.
 * 
 *      \param T permet de choisir le type de l'élément data
 *  \param N permet de choisir la taille du tableau d'objet du message 
 *  Si N = 0 : data est une structure de type T
 *  Si N > 0 : data est un tableau d'élement de type T et de taille N
 */



struct timestamp{
        uint32_t sec=0;
        uint32_t nanosec=0;

        void now(){
                struct timespec now;
                timespec_get( &now, TIME_UTC );
                sec = (int32_t)now.tv_sec;
                nanosec = (uint32_t)now.tv_nsec;
        }

        uint64_t toNanosec(){
                return (uint64_t)((uint64_t)sec*1000000000 + (uint64_t)nanosec) ;
        }
};

inline std::ostream &operator<<(std::ostream &out, timestamp stamp) {
    out << "[" << stamp.sec << '.' << stamp.nanosec << ']';
    return out;
}

inline timestamp operator-(timestamp const& a, timestamp const& b)
{
    timestamp resultat;
    resultat.sec = a.sec - b.sec;
        resultat.nanosec = a.nanosec - b.nanosec;
    return resultat;
}

//message ipc

template <typename T>
struct ipc_message {
        long msg_type = 1;
        struct {
                struct timestamp stamp;
                T value;
        } data;

        void time_now(){
                data.stamp.now();
        }
};

template <typename T>
struct ipc_tab {
        long msg_type = 1;
        struct {
                struct timestamp stamp;
                T values[1];
        } data;

        void time_now(){
                data.stamp.now();
        }
};

} // end namespace type
} // end namespace ipc



#endif