mirror of https://github.com/ArduPilot/ardupilot
69 lines
1.3 KiB
C++
69 lines
1.3 KiB
C++
|
#include "float16.h"
|
||
|
|
/*
|
||
|
|
float16 implementation
|
||
|
|
|
||
|
|
algorithm with thanks to libcanard:
|
||
|
|
https://github.com/dronecan/libcanard
|
||
|
|
*/
|
||
|
|
|
||
|
|
|
||
|
|
float Float16_t::get(void) const
|
||
|
|
{
|
||
|
|
union FP32 {
|
||
|
|
uint32_t u;
|
||
|
|
float f;
|
||
|
|
};
|
||
|
|
const union FP32 magic = { (254UL - 15UL) << 23U };
|
||
|
|
const union FP32 was_inf_nan = { (127UL + 16UL) << 23U };
|
||
|
|
union FP32 out;
|
||
|
|
|
||
|
|
out.u = (v16 & 0x7FFFU) << 13U;
|
||
|
|
out.f *= magic.f;
|
||
|
|
if (out.f >= was_inf_nan.f) {
|
||
|
|
out.u |= 255UL << 23U;
|
||
|
|
}
|
||
|
|
out.u |= (v16 & 0x8000UL) << 16U;
|
||
|
|
|
||
|
|
return out.f;
|
||
|
|
}
|
||
|
|
|
||
|
|
void Float16_t::set(float value)
|
||
|
|
{
|
||
|
|
union FP32
|
||
|
|
{
|
||
|
|
uint32_t u;
|
||
|
|
float f;
|
||
|
|
};
|
||
|
|
|
||
|
|
const union FP32 f32inf = { 255UL << 23U };
|
||
|
|
const union FP32 f16inf = { 31UL << 23U };
|
||
|
|
const union FP32 magic = { 15UL << 23U };
|
||
|
|
const uint32_t sign_mask = 0x80000000UL;
|
||
|
|
const uint32_t round_mask = 0xFFFFF000UL;
|
||
|
|
|
||
|
|
union FP32 in;
|
||
|
|
in.f = value;
|
||
|
|
uint32_t sign = in.u & sign_mask;
|
||
|
|
in.u ^= sign;
|
||
|
|
|
||
|
|
v16 = 0;
|
||
|
|
|
||
|
|
if (in.u >= f32inf.u)
|
||
|
|
{
|
||
|
|
v16 = (in.u > f32inf.u) ? (uint16_t)0x7FFFU : (uint16_t)0x7C00U;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
in.u &= round_mask;
|
||
|
|
in.f *= magic.f;
|
||
|
|
in.u -= round_mask;
|
||
|
|
if (in.u > f16inf.u)
|
||
|
|
{
|
||
|
|
in.u = f16inf.u;
|
||
|
|
}
|
||
|
|
v16 = (uint16_t)(in.u >> 13U);
|
||
|
|
}
|
||
|
|
|
||
|
|
v16 |= (uint16_t)(sign >> 16U);
|
||
|
|
}
|