fix velocity actual readout

hacknus · Apr 24, 2024 · ddaca04 · ddaca04
1 parent 57d3cfd
commit ddaca04
Showing 1 changed file with 7 additions and 8 deletions.
diff --git a/src/lib.rs b/src/lib.rs
@@ -610,14 +610,13 @@ impl<SPI, CS, EN, E> Tmc5160<SPI, CS, EN>
 
     /// get the current velocity
     pub fn get_velocity(&mut self) -> Result<f32, Error<E>> {
-        // self.read_register(Registers::VACTUAL).map(|target| {
-        //     if (target.data & 0b100000000000000000000000) == 0b100000000000000000000000 {
-        //         ((16777216 - target.data as i32) as f64 / self._step_count as f64) as f32
-        //     } else {
-        //         ((target.data as i32) as f64 / self._step_count as f64) as f32
-        //     }
-        // })
-        self.read_register(Registers::VACTUAL).map(|val| (val.data as i32) as f32 / self._step_count)
+        self.read_register(Registers::VACTUAL).map(|val| {
+            if (val.data & 0b1000000000000000000000000) == 0b1000000000000000000000000 {
+                ((8388607 - val.data as i32) as f64 / self._step_count as f64) as f32
+            } else {
+                ((val.data as i32) as f64 / self._step_count as f64) as f32
+            }
+        })
     }
 
     /// get the set maximum velocity (VMAX)