Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

Utils.h

Committer:
pvaibhav
Date:
2015-05-26
Revision:
40:8e852115fe55
Child:
46:fd5a62296b12

File content as of revision 40:8e852115fe55:

#ifndef _H_UTILS_H
#define _H_UTILS_H

#include "Vector3.h"

class utils
{
public:
    static float clip(float const minimum, float const val, float const maximum)
    {
        return (val < minimum) ? minimum : ((val > maximum) ? maximum : val);
    }
    
    static void debugPortSend(uint8_t const* src, size_t const size)
    {
    #if defined(DATA_OUTPUT)
        putc(0x10, stdout); // marker
        for (int i = 0; i < size; i++) {
            putc(src[i] & 0x0f, stdout);
            putc(src[i]   >> 4, stdout);
        }
    #endif
    }

    static void sendVectorOverUART( const Vector3 a = Vector3(0, 0, 0),
                                    const Vector3 b = Vector3(0, 0, 0),
                                    const Vector3 c = Vector3(0, 0, 0),
                                    const Vector3 d = Vector3(0, 0, 0))
    {
        float data[] = {
            a.x, a.y, a.z,
            b.x, b.y, b.z,
            c.x, c.y, c.z,
            d.x, d.y, d.z
        };
        
        debugPortSend((const uint8_t*) data, sizeof data);
    }
    
    static float deg2rad(float const d)
    {
        return d * 0.0174532925f;
    }
    
    static float rad2deg(float const r)
    {
        return r * 57.2957795131f;
    }
};

#endif//_H_UTILS_H