Decode CAN signals

signaldecode.c

#include <stdint.h>
#include <iostream>

#include <stdint.h> //uint typedefinitions, non-rtw!

#define MASK64(nbits) ((0xffffffffffffffff)>> (64-nbits))

float toPhysicalValue(uint64_t target, float factor, float offset, bool is_signed)
{
    if (is_signed)
        return ( (int64_t) target ) * factor + offset;
    else
        return target * factor + offset;
}

uint64_t fromPhysicalValue(float physical_value, float factor, float offset)
{
    return (physical_value - offset) / factor;
}

void storeSignal( uint8_t* frame, uint64_t value, const uint8_t startbit, const uint8_t length, bool is_big_endian)
{
    uint8_t start_byte = startbit / 8;
    uint8_t startbit_in_byte = startbit % 8;
    uint8_t end_byte = 0;
    int8_t count = 0;
    uint8_t current_target_length = (8-startbit_in_byte);

    value &= MASK64(length);

    frame[start_byte] |= value >> startbit_in_byte;
    if(is_big_endian) // Motorola (big endian)
    {
        end_byte = (start_byte * 8 + 8 - startbit_in_byte - length) / 8;

        for(count = start_byte-1; count >= end_byte; count --)
        {
            frame[count] |= value << current_target_length;
            current_target_length += 8;
        }
    }
    else // Intel (little endian)
    {
        end_byte = (startbit + length - 1) / 8;
        for(count = start_byte+1; count <= end_byte; count ++)
        {
            frame[count] |= value << current_target_length;
            current_target_length += 8;
        }
    }
}


uint64_t extractSignal(const uint8_t* frame, const uint8_t startbit, const uint8_t length, bool is_big_endian)
{
    uint8_t start_byte = startbit / 8;
    uint8_t startbit_in_byte = startbit % 8;
    uint8_t end_byte = 0;
    int8_t count = 0;
    uint64_t target = frame[start_byte] >> startbit_in_byte;
    uint8_t current_target_length = (8-startbit_in_byte);

    if(is_big_endian) // Motorola (big endian)
    {
        end_byte = (start_byte * 8 + 8 - startbit_in_byte - length) / 8;

        for(count = start_byte-1; count >= end_byte; count --)
        {
            target |= frame[count] << current_target_length;
            current_target_length += 8;
        }
    }
    else // Intel (little endian)
    {
        end_byte = (startbit+length) / 8;

        for(count = start_byte+1; count < end_byte; count ++)
        {
            target |= frame[count] << current_target_length;
            current_target_length += 8;
        }
    }

    target &= MASK64(length);
    return target;
}

float decode(const uint8_t* frame, const uint16_t startbit, const uint16_t length, bool is_big_endian, bool is_signed, float factor, float offset)
{
    return toPhysicalValue(extractSignal(frame, startbit, length, is_big_endian), factor, offset, is_signed);
}

void test(uint8_t* frame, uint8_t startbit, uint8_t length, bool is_big_endian, bool is_signed, float factor, float offset, uint32_t int_value, float value)
{
    std::cout << "\n---- (startbit=" << (int) startbit << ",length=" << (int) length << ",is_big_endian=" << is_big_endian << ",is_signed=" << is_signed << ",factor=" << factor << ",offset=" << offset << ")" << std::endl;
    std::cout << "Unsigned int value: " << extractSignal(frame, startbit, length, is_big_endian) << " should be " << int_value << std::endl;
    std::cout << "Physical value:     " << decode(frame, startbit, length, is_big_endian, is_signed, factor, offset) << " should be " << value << std::endl;
}

int main(void)
{
    {
        uint8_t src_array[8] = {48, 48, 48, 48, 225, 127, 202, 139};
        test(src_array, 34, 2, true, false, 1.000000, 0, 0, 0.000000);
        test(src_array, 40, 8, false, false, 0.010000, 0, 127, 1.270000);
        test(src_array, 32, 1, false, false, 1.000000, 0, 1, 1.000000);
        test(src_array, 16, 16, false, false, 0.003906, 0, 12336, 48.187500);
        test(src_array, 0, 16, false, false, 0.000977, -31, 12336, -19.952148);
    }

    {
        uint8_t src_array[8] = {13, 27, 78, 68, 254, 139, 13, 147};
        test(src_array, 0, 2, true, false, 1.000000, 0, 1, 1.000000);
        test(src_array, 2, 6, true, false, 1.000000, 0, 3, 3.000000);
        test(src_array, 21, 11, true, false, 0.100000, 0, 218, 21.800000);
        test(src_array, 25, 12, true, false, 0.062500, -128, 1826, -13.875000);
        test(src_array, 32, 9, true, false, 0.062500, -16, 254, -0.125000);
        test(src_array, 48, 3, true, false, 1.000000, 0, 5, 5.000000);
        test(src_array, 51, 3, true, false, 1.000000, 0, 1, 1.000000);
        test(src_array, 54, 10, true, false, 0.100000, -52, 556, 3.600000);
        test(src_array, 56, 3, true, false, 1.000000, 0, 3, 3.000000);
        test(src_array, 59, 3, true, false, 1.000000, 0, 2, 2.000000);
        test(src_array, 62, 2, true, false, 1.000000, 0, 2, 2.000000);
    }

    return 0;
}