uc_float_spec.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <uavcan/marshal/float_spec.hpp>
00006 #include <uavcan/build_config.hpp>
00007 #include <cmath>
00008 
00009 namespace uavcan
00010 {
00011 
00012 #if !UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION
00013 
00014 union Fp32
00015 {
00016     uint32_t u;
00017     float f;
00018 };
00019 
00020 /*
00021  * IEEE754Converter
00022  */
00023 uint16_t IEEE754Converter::nativeIeeeToHalf(float value)
00024 {
00025     /*
00026      * https://gist.github.com/rygorous/2156668
00027      * Public domain, by Fabian "ryg" Giesen
00028      */
00029     const Fp32 f32infty = { 255U << 23 };
00030     const Fp32 f16infty = { 31U << 23 };
00031     const Fp32 magic = { 15U << 23 };
00032     const uint32_t sign_mask = 0x80000000U;
00033     const uint32_t round_mask = ~0xFFFU;
00034 
00035     Fp32 in;
00036     uint16_t out;
00037 
00038     in.f = value;
00039 
00040     uint32_t sign = in.u & sign_mask;
00041     in.u ^= sign;
00042 
00043     if (in.u >= f32infty.u) /* Inf or NaN (all exponent bits set) */
00044     {
00045         /* NaN->sNaN and Inf->Inf */
00046         out = (in.u > f32infty.u) ? 0x7FFFU : 0x7C00U;
00047     }
00048     else /* (De)normalized number or zero */
00049     {
00050         in.u &= round_mask;
00051         in.f *= magic.f;
00052         in.u -= round_mask;
00053         if (in.u > f16infty.u)
00054         {
00055             in.u = f16infty.u; /* Clamp to signed infinity if overflowed */
00056         }
00057 
00058         out = uint16_t(in.u >> 13); /* Take the bits! */
00059     }
00060 
00061     out = uint16_t(out | (sign >> 16));
00062 
00063     return out;
00064 }
00065 
00066 float IEEE754Converter::halfToNativeIeee(uint16_t value)
00067 {
00068     /*
00069      * https://gist.github.com/rygorous/2144712
00070      * Public domain, by Fabian "ryg" Giesen
00071      */
00072     const Fp32 magic = { (254U - 15U) << 23 };
00073     const Fp32 was_infnan = { (127U + 16U) << 23 };
00074     Fp32 out;
00075 
00076     out.u = (value & 0x7FFFU) << 13;   /* exponent/mantissa bits */
00077     out.f *= magic.f;                  /* exponent adjust */
00078     if (out.f >= was_infnan.f)         /* make sure Inf/NaN survive */
00079     {
00080         out.u |= 255U << 23;
00081     }
00082     out.u |= (value & 0x8000U) << 16;  /* sign bit */
00083 
00084     return out.f;
00085 }
00086 
00087 #endif // !UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION
00088 
00089 }