Velocity.hxx

Go to the documentation of this file.

 
#ifndef _OPENLCB_VELOCITY_HXX_
#define _OPENLCB_VELOCITY_HXX_
 
#include <cmath>
#include <cstdint>
 
#include "utils/macros.h"
 
extern "C" {
/* These come from the ieeehalfprecision.c */
int singles2halfp(void *target, const void *source, int numel);
int halfp2singles(void *target, const void *source, int numel);
}
 
#define MPH_FACTOR 0.44704f
 
typedef uint16_t float16_t;
 
namespace openlcb
{
 
class Velocity
{
public:
    enum
    {
        FORWARD  = false, 
        REVERSE  = true, 
    };
 
    Velocity()
        : velocity(0)
    {
    }
 
    Velocity(int value)
        : velocity(value)
    {
    }
 
    Velocity(unsigned int value)
        : velocity(value)
    {
    }
 
    Velocity(float value)
        : velocity(value)
    {
    }
 
    Velocity(double value)
        : velocity(value)
    {
    }
 
    Velocity(float16_t value)
        : velocity(0)
    {
        halfp2singles(&velocity, &value, 1);
    }
 
    Velocity(const Velocity& old_velocity)
        : velocity(old_velocity.velocity)
    {
    }
 
    ~Velocity() {}
 
    static Velocity from_mph(float mph) {
        Velocity v;
        v.set_mph(mph);
        return v;
    }
 
    bool isnan() const
    {
        return std::isnan(velocity);
    }
 
    float speed() const
    {
        return fabsf(velocity);
    }
 
    bool direction() const
    {
        if (std::signbit(velocity))
        {
            return REVERSE;
        }
        return FORWARD;
    }
    
    void set_direction(bool direction)
    {
        if (direction == FORWARD)
        {
            forward();
        }
        else
        {
            reverse();
        }
    }
 
    void forward()
    {
        if (std::signbit(velocity))
        {
            velocity = -velocity;
        }
    }
    
    void reverse()
    {
        if (!std::signbit(velocity))
        {
            velocity = -velocity;
        }
    }
    
    float mph() const
    {
        return speed() / MPH_FACTOR;
    }
 
    void set_mph(float mph)
    {
        // should this be std::copysign? armgcc says not.
        velocity = copysign(mph * MPH_FACTOR, velocity);
    }
    
    uint8_t get_dcc_128();
 
    void set_dcc_128(uint8_t value);
    
    uint8_t get_dcc_28();
    
    void set_dcc_28(uint8_t value);
 
    uint8_t get_dcc_14();
    
    void set_dcc_14(uint8_t value);
    
    float16_t get_wire() const
    {
        float16_t result;
        singles2halfp(&result, &velocity, 1);
        return result;
    }
    
    void set_wire(float16_t value)
    {
        halfp2singles(&velocity, &value, 1);
    }
 
    Velocity operator + (const Velocity& v) const
    {
        return Velocity(zero_adjust(velocity + v.velocity,velocity));
    }
 
    Velocity operator + (const float& v) const
    {
        return Velocity(zero_adjust(velocity + v,velocity));
    }
 
    Velocity operator - (const Velocity& v) const
    {
        return Velocity(zero_adjust(velocity - v.velocity,velocity));
    }
 
    Velocity operator - (const float& v) const
    {
        return Velocity(zero_adjust(velocity - v,velocity));
    }
 
    Velocity operator * (const Velocity& v) const
    {
        return Velocity(zero_adjust(velocity * v.velocity,velocity));
    }
 
    Velocity operator * (const float& v) const
    {
        return Velocity(zero_adjust(velocity * v,velocity));
    }
 
    Velocity operator / (const Velocity& v) const
    {
        return Velocity(zero_adjust(velocity / v.velocity,velocity));
    }
 
    Velocity operator / (const float& v) const
    {
        return Velocity(zero_adjust(velocity / v,velocity));
    }
 
    Velocity& operator ++ ()
    {
        velocity = zero_adjust(velocity + 1,velocity);
        return *this;
    }
 
    Velocity operator ++ (int)
    {
        Velocity result(*this);
        velocity = zero_adjust(velocity + 1,velocity);
        return result;
    }
 
    Velocity& operator -- ()
    {
        velocity = zero_adjust(velocity - 1,velocity);
        return *this;
    }
 
    Velocity operator -- (int)
    {
        Velocity result(*this);
        velocity = zero_adjust(velocity - 1,velocity);
        return result;
    }
 
    Velocity& operator += (const Velocity& v)
    {
        velocity = zero_adjust(velocity + v.velocity,velocity);
        return *this;
    }
 
    Velocity& operator += (const float& v)
    {
        velocity = zero_adjust(velocity + v,velocity);
        return *this;
    }
 
    Velocity& operator -= (const Velocity& v)
    {
        velocity = zero_adjust(velocity - v.velocity,velocity);
        return *this;
    }
 
    Velocity& operator -= (const float& v)
    {
        velocity = zero_adjust(velocity - v,velocity);
        return *this;
    }
 
    Velocity& operator *= (const Velocity& v)
    {
        velocity = zero_adjust(velocity * v.velocity,velocity);
        return *this;
    }
 
    Velocity& operator *= (const float& v)
    {
        velocity = zero_adjust(velocity * v,velocity);
        return *this;
    }
 
    Velocity& operator /= (const Velocity& v)
    {
        velocity = zero_adjust(velocity / v.velocity,velocity);
        return *this;
    }
 
    Velocity& operator /= (const float& v)
    {
        velocity = zero_adjust(velocity / v,velocity);
        return *this;
    }
 
    Velocity& operator = (const Velocity& v)
    {
        velocity = v.velocity;
        return *this;
    }
 
    Velocity& operator = (const float& v)
    {
        velocity = v;
        return *this;
    }
 
    bool operator == (const Velocity& v) const
    {
        return (v.velocity == velocity);
    }
    
    bool operator == (const float& v) const
    {
        return (v == velocity);
    }
    
    bool operator != (const Velocity& v) const
    {
        return (v.velocity != velocity);
    }
    
    bool operator != (const float& v) const
    {
        return (v != velocity);
    }
    
private:
    float velocity;
    
    static float zero_adjust(float value, float old)
    {
        if (value == 0)
        {
            return copysign(value, old);
        }
        return value;
    }
};
 
inline Velocity nan_to_speed()
{
    Velocity s;
    s.set_wire(0xFFFFU);
    return s;
}
 
}; /* namespace openlcb */
 
#endif // _OPENLCB_VELOCITY_HXX_