utilities.h

Go to the documentation of this file.

//   Copyright 2016-2023 Jean-Francois Poilpret
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
 
 
#ifndef UTILITIES_HH
#define UTILITIES_HH
 
#include "defines.h"
#include "boards/board.h"
#include <util/atomic.h>
 
#define synchronized _Pragma("GCC diagnostic ignored \"-Wreturn-type\"") ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
 
namespace utils
{
    template<typename T> constexpr T constrain(T value, T min, T max)
    {
        if (value < min)
            return min;
        else if (value > max)
            return max;
        else
            return value;
    }
 
    template<typename TI, typename TO>
    constexpr TO map(TI value, TI input_min, TI input_max, TO output_min, TO output_max)
    {
        return output_min + (value - input_min) * (output_max - output_min) / (input_max - input_min);
    }
 
    // NOTE: we use prefixes that can be covered in a uint32
    enum class UnitPrefix : int8_t
    {
        GIGA = 9,
        MEGA = 6,
        KILO = 3,
        HECTO = 2,
        DECA = 1,
        NONE = 0,
        DECI = -1,
        CENTI = -2,
        MILLI = -3,
        MICRO = -6,
        NANO = -9,
    };
 
    template<typename T> constexpr T min(T a, T b)
    {
        return (a < b ? a : b);
    }
 
    template<typename T> constexpr T max(T a, T b)
    {
        return (a > b ? a : b);
    }
 
    constexpr uint32_t power_of_10(int8_t n)
    {
        if (n > 0)
            return 10UL * power_of_10(n - 1);
        else if (n < 0)
            return power_of_10(-n);
        else
            return 1;
    }
 
    constexpr int16_t map_raw_to_physical(int16_t value, UnitPrefix prefix, int16_t range, uint8_t precision_bits)
    {
        // Here we approximate the calculation by using 2^n instead of (2^n - 1) as input range
        const int8_t prefix_value = int8_t(prefix);
        if (prefix_value > 0)
            return (int32_t(value) * int32_t(range) / int32_t(power_of_10(prefix_value))) >> precision_bits;
        else
            return (int32_t(value) * int32_t(range) * int32_t(power_of_10(prefix_value))) >> precision_bits;
    }
 
    // This is intermediate function used by map_physical_to_raw() but without overflow issue (int32 only)
    constexpr int32_t map_physical_to_raw_(int32_t value, int8_t prefix, int32_t range, uint8_t precision_bits)
    {
        // Here we approximate the calculation by using 2^n instead of (2^n - 1) as input range
        if (prefix >= 0)
            return (value << precision_bits) * int32_t(power_of_10(prefix)) / range;
        else
            return (value << precision_bits) / int32_t(power_of_10(prefix)) / range;
    }
 
    constexpr int16_t map_physical_to_raw(int16_t value, UnitPrefix prefix, int16_t range, uint8_t precision_bits)
    {
        // We first perform calculation as int32
        const int32_t output = map_physical_to_raw_(value, int8_t(prefix), range, precision_bits);
        //Then we deal with boundary cases before conversion to int16
        if (output > INT16_MAX)
            return INT16_MAX;
        else if (output <= INT16_MIN)
            return INT16_MIN;
        else
            return output;
    }
 
    constexpr uint8_t low_byte(uint16_t word)
    {
        return uint8_t(word & 0xFF);
    }
 
    constexpr uint8_t high_byte(uint16_t word)
    {
        return uint8_t(word >> 8);
    }
 
    constexpr uint16_t as_uint16_t(uint8_t high, uint8_t low)
    {
        return uint16_t(uint16_t(high) << 8) | uint16_t(low);
    }
 
    template<typename T> constexpr T is_zero(T value, T default_value)
    {
        return (value ? value : default_value);
    }
 
    template<typename T> void set_mask(volatile T& reg, T mask, T value)
    {
        reg = (reg & ~mask) | (value & mask);
    }
 
    template<typename T> constexpr bool is_mask_equal(T actual, T mask, T expected)
    {
        return (actual & mask) == (expected & mask);
    }
 
    inline uint8_t bcd_to_binary(uint8_t bcd)
    {
        const uint8_t tens = bcd / 16;
        // We avoid tens * 10 to avoid adding library for multiplication
        return (tens << 3) + (tens << 1) + (bcd & 0x0F);
    }
 
    inline uint8_t binary_to_bcd(uint8_t binary)
    {
        uint8_t bcd = 0;
        while (binary >= 10)
        {
            bcd += 0x10;
            binary -= 10;
        }
        return bcd + binary;
    }
 
    inline void swap_bytes(uint16_t& value)
    {
        value = (value >> 8) | (value << 8);
    }
 
    inline void swap_bytes(int16_t& value)
    {
        swap_bytes((uint16_t&) value);
    }
 
    inline void swap_bytes(uint32_t& value)
    {
        uint16_t value1 = uint16_t(value >> 16);
        uint16_t value2 = uint16_t(value & 0xFFFFU);
        swap_bytes(value1);
        swap_bytes(value2);
        value = uint32_t(value1) | (uint32_t(value2) << 16);
    }
 
    inline void swap_bytes(int32_t& value)
    {
        swap_bytes((uint32_t&) value);
    }
 
    inline void swap_bytes(uint64_t& value)
    {
        uint32_t value1 = uint32_t(value >> 32);
        uint32_t value2 = uint32_t(value & 0xFFFF'FFFFUL);
        swap_bytes(value1);
        swap_bytes(value2);
        value = uint64_t(value1) | (uint64_t(value2) << 32);
    }
 
    inline void swap_bytes(int64_t& value)
    {
        swap_bytes((uint64_t&) value);
    }
 
    template<typename T> void swap(T& a, T&b)
    {
        T c = a;
        a = b;
        b = c;
    }
 
    template<typename T> union ToUint8
    {
        static_assert(sizeof(T) == 1, "T must be a one-byte size type");
        explicit ToUint8(T value) : value_(value) {}
        T value_;
        uint8_t as_uint8_;
    };
 
    template<typename T> union ToArray
    {
        explicit ToArray(const T& value): value_{value} {}
        T value_;
        uint8_t as_array_[sizeof(T)];
    };
 
    template<typename T> constexpr uint8_t as_uint8_t(T input)
    {
        return ToUint8<T>(input).as_uint8_;
    }
 
    template<typename T, typename U = uint8_t[sizeof(T)]>
    constexpr void as_array(const T& input, U output)
    {
        memcpy(output, ToArray<T>(input).as_array_, sizeof(T));
    } 
 
    constexpr uint8_t calculate_delay1_count(float time_us)
    {
        return uint8_t(INST_PER_US / 3.0 * time_us);
    }
 
    constexpr uint8_t num_bits(uint8_t mask, uint8_t num = 0)
    {
        if (mask == 0)
            return num;
        else if (mask & 1)
            return num_bits(mask >> 1, num + 1);
        else
            return num_bits(mask >> 1, num);
    }
 
    template<typename T> T inline change_endianness(const T& value)
    {
        return value;
    }
    
    template<> uint16_t inline change_endianness(const uint16_t& value)
    {
        uint16_t temp = value;
        utils::swap_bytes(temp);
        return temp;
    }
    template<> int16_t inline change_endianness(const int16_t& value)
    {
        int16_t temp = value;
        utils::swap_bytes(temp);
        return temp;
    }
    template<> uint32_t inline change_endianness(const uint32_t& value)
    {
        uint32_t temp = value;
        utils::swap_bytes(temp);
        return temp;
    }
    template<> int32_t inline change_endianness(const int32_t& value)
    {
        int32_t temp = value;
        utils::swap_bytes(temp);
        return temp;
    }
}
 
#endif /* UTILITIES_HH */