Diff: Utility.cpp

Revision:

13:bb9669053eb3

Parent:

12:1c341b119b23

Child:

15:b38d9d210e32

--- a/Utility.cpp	Sun Jan 15 19:46:10 2017 +0000
+++ b/Utility.cpp	Mon Jan 16 00:06:45 2017 +0000
@@ -22,13 +22,22 @@
         return wrap_angle(angle_degree);
     }
     
-    int update_bit(const int previous_4_bytes, const int position, const bool new_bit_value)
+    unsigned int update_bit(const unsigned int previous_4_bytes, const int position, const bool new_bit_value)
+    {
+        return update_bits(previous_4_bytes, position, position, new_bit_value);
+    }
+    
+    unsigned int update_bits(const unsigned int previous_4_bytes, const int start_bit, const int stop_bit, const unsigned int new_bits)
     {
-        const int all_zero_but_one_at_position = 0x1 << position;
-        const int all_one_but_zero_at_position = ~all_zero_but_one_at_position;
-        const int all_zero_but_new_bit_value_at_position = new_bit_value << position;
-        const int all_unchanged_but_zero_at_position = previous_4_bytes & all_one_but_zero_at_position;
-        const int all_unchanged_but_new_bit_value_at_position = all_unchanged_but_zero_at_position | all_zero_but_new_bit_value_at_position;
+        const int all_ones_but_n_zeros_right_shifted = (0xFFFFFFFF << (stop_bit - start_bit + 1));
+        const int all_zeros_but_n_ones_right_shifted = ~all_ones_but_n_zeros_right_shifted;
+        
+        const int all_zeros_but_ones_at_position = all_zeros_but_n_ones_right_shifted << start_bit;
+        const int all_ones_but_zeros_at_position = ~all_zeros_but_ones_at_position;
+        
+        const int all_zeros_but_new_bits_at_position = new_bits << start_bit;
+        const int all_unchanged_but_zeros_at_position = previous_4_bytes & all_ones_but_zeros_at_position;
+        const int all_unchanged_but_new_bit_value_at_position = all_unchanged_but_zeros_at_position | all_zeros_but_new_bits_at_position;
         return all_unchanged_but_new_bit_value_at_position;
     }
 }
\ No newline at end of file