USNA WSE ES456 / Mbed 2 deprecated Emaxx_Navigation_madpulse

Emaxx Navigation code ported for MBED

Dependencies:   BNO055_fusion Emaxx_Navigation_Dynamic_HIL MODSERIAL ServoIn ServoOut Vehicle_Model mbed

Fork of Emaxx_Navigation_Dynamic_HIL by Emaxx Navigation Group

utilityFunctions.h

Committer:
jdawkins
Date:
2016-12-16
Revision:
7:a8c2e9d049e8

File content as of revision 7:a8c2e9d049e8:

#ifndef UTIL_FUNC_H
#define UTIL_FUNC_H

#define Pi  (3.14159)
#define DEG2RAD (Pi/180)
#define RAD2DEG (180/Pi)
float saturateCmd(float cmd, float max,float min)
{
    if(cmd>max) {
        cmd = max;
    }
    if(cmd < min) {
        cmd = min;
    }
    return cmd;
}
float saturateCmd(float cmd)
{
    if(cmd>1.0) {
        cmd = 1.0;
    }
    if(cmd < -1.0) {
        cmd = -1.0;
    }
    return cmd;
}
float wrapToPi(float ang)
{

    while(ang > Pi) {

        ang = ang - 2*Pi;
    }
    while(ang < -Pi) {
        ang = ang + 2*Pi;
    }

    return ang;
}

#endif