fix: round joint positions to nearest integer

srvald · srvald · commit f66419339451 · 2026-05-20T11:46:08.000+02:00
diff --git a/src/control/reverse_interface.cpp b/src/control/reverse_interface.cpp
@@ -209,7 +209,7 @@ bool ReverseInterface::writeFreedriveControlMessage(const FreedriveControlMessag
   // Add feature pose scaled to microns microradians to avoid precision loss in integer conversion
   for (double p : feature_pose)
   {
-    val = htobe32(static_cast<int32_t>(p * MULT_JOINTSTATE));
+    val = htobe32(static_cast<int32_t>(round(p * MULT_JOINTSTATE)));
     b_pos += append(b_pos, val);
   }
 

Original file line number	Diff line number	Diff line change
`@@ -209,7 +209,7 @@ bool ReverseInterface::writeFreedriveControlMessage(const FreedriveControlMessag`
`209`	`209`	`// Add feature pose scaled to microns microradians to avoid precision loss in integer conversion`
`210`	`210`	`for (double p : feature_pose)`
`211`	`211`	`{`
`212`		`- val = htobe32(static_cast<int32_t>(p * MULT_JOINTSTATE));`
	`212`	`+ val = htobe32(static_cast<int32_t>(round(p * MULT_JOINTSTATE)));`
`213`	`213`	`b_pos += append(b_pos, val);`
`214`	`214`	`}`
`215`	`215`