converter: Fix to_hex conversion

[apps/agl-service-can-low-level.git] / low-can-binding / can / can-decoder.cpp

diff --git a/low-can-binding/can/can-decoder.cpp b/low-can-binding/can/can-decoder.cpp
index 7e60051..f38840e 100644 (file)
--- a/low-can-binding/can/can-decoder.cpp
+++ b/low-can-binding/can/can-decoder.cpp
@@ -111,12 +111,6 @@ float decoder_t::parse_signal_bitfield(signal_t& signal, std::shared_ptr<message
 
        int sign = handle_sign(signal, data_signal, new_end_bit, data);
 
-//     if(bit_size > 255)
-//             AFB_ERROR("Error signal %s to long bit size", signal.get_name().c_str());
-
-//     if(new_start_bit > 255)
-//             AFB_ERROR("Too long signal offset %d", new_start_bit);
-
        if(data_signal.size() > 65535)
                AFB_ERROR("Too long data signal %s", signal.get_name().c_str());
 
@@ -146,8 +140,9 @@ openxc_DynamicField decoder_t::decode_bytes(signal_t& signal, std::shared_ptr<me
        uint32_t length = message->get_length();
        uint32_t bit_position = signal.get_bit_position();
        uint32_t bit_size = signal.get_bit_size();
+
        std::vector<uint8_t> new_data = std::vector<uint8_t>();
-       new_data.reserve(bit_size << 3);
+       new_data.reserve((bit_size / 8) + 1);
 
        int new_start_byte = 0;
        int new_end_byte = 0;
@@ -157,59 +152,29 @@ openxc_DynamicField decoder_t::decode_bytes(signal_t& signal, std::shared_ptr<me
        converter_t::signal_to_bits_bytes(bit_position, bit_size, new_start_byte, new_end_byte, new_start_bit, new_end_bit);
 
        if(new_end_byte >= length)
-       {
                new_end_byte = length-1;
-       }
 
        if(new_start_byte >= length)
        {
-               AFB_ERROR("Error in description of signals");
+               AFB_ERROR("Error in signal's description");
                return decoded_value;
        }
 
-       uint8_t first = data[new_start_byte];
-       int mask_first = 0;
-       for(i=new_start_bit;i<8;i++)
-       {
-               mask_first = mask_first | (1 << i);
-       }
-
-       uint8_t mask_first_v = 0;
-       if(mask_first > 255)
-       {
-               AFB_ERROR("Error mask decode bytes");
-       }
-       else
-       {
-               mask_first_v = (uint8_t)mask_first;
-       }
-
-       data[new_start_byte]=first&mask_first_v;
-
-       uint8_t last = data[new_end_byte];
-       int mask_last = 0;
-       for(i=0;i<=new_end_bit;i++)
-       {
-               mask_last = mask_last | (1 << (7-i));
-       }
+       uint8_t mask_first_v = static_cast<uint8_t>(0xFF << new_start_bit);
+       uint8_t mask_last_v = static_cast<uint8_t>(0xFF >> (7 - new_end_bit));
 
-       uint8_t mask_last_v = 0;
-       if(mask_last > 255)
+       if(new_start_byte == new_end_byte)
        {
-               AFB_ERROR("Error mask decode bytes");
+               data[new_start_byte] = data[new_start_byte] & (mask_first_v & mask_last_v);
        }
        else
        {
-               mask_last_v = (uint8_t)mask_last;
+               data[new_start_byte] = data[new_start_byte] & mask_first_v;
+               data[new_end_byte] = data[new_end_byte] & mask_last_v;
        }
 
-       data[new_end_byte]=last&mask_last_v;
-
-
-       for(i=new_start_byte;i<=new_end_byte;i++)
-       {
+       for(i=new_start_byte ; i <= new_end_byte ; i++)
                new_data.push_back(data[i]);
-       }
 
        decoded_value = build_DynamicField(new_data);
 
@@ -267,9 +232,8 @@ openxc_DynamicField decoder_t::decode_boolean(signal_t& signal, std::shared_ptr<
 
        // Don't send if they is no changes
        if ((signal.get_last_value() == value && !signal.get_send_same()) || !*send )
-       {
                *send = false;
-       }
+
        signal.set_last_value(value);