impl Stored_U26 constructor

lib-bms-protocol/src/types.rs

@@ -48,7 +48,7 @@ const U26_VALUE_MASK: u32 = 0xFFFFFFc0;
 const U26_MAX_VALUE: u32 = 0x03FFFFFF;
 
 pub enum U26Error {
-    Overflow
+    Overflow,
 }
 
 #[derive(Debug)]
@@ -62,6 +62,14 @@ pub enum U26Error {
 /// | 26 bits u26 value | 5 hemming bits | 1 unused extrabit |
 pub struct Stored_U26(u32);
 
+impl Stored_U26 {
+    pub fn new(value: u32) -> Self {
+        assert!(value <= U26_MAX_VALUE);
+        let hemming_code = calc_hemming(value);
+        Self((value << 6) | ((hemming_code & 0x3F) << 1) as u32)
+    }
+}
+
 impl From<Stored_U26> for u32 {
     fn from(value: Stored_U26) -> Self {
         value.0 & U26_VALUE_MASK >> 6
@@ -75,7 +83,7 @@ impl TryFrom<u32> for Stored_U26 {
         if value > U26_MAX_VALUE {
             Err(U26Error::Overflow)
         } else {
-            todo!()
+            Ok(Self::new(value))
         }
     }
 }