From: LeonardoBizzoni <leo2002714@gmail.com>
Date: Wed, 22 Oct 2025 20:28:07 +0000 (+0200)
Subject: upgraded `Odometry` to C
X-Git-Url: http://git.leonardobizzoni.com/?a=commitdiff_plain;h=3aaccc26491272d501c65760e561f4caeabe2595;p=pioneer-stm32

upgraded `Odometry` to C
---

diff --git a/otto_controller/Core/Inc/control/odometry.h b/otto_controller/Core/Inc/control/odometry.h
index 417f062..4669af8 100644
--- a/otto_controller/Core/Inc/control/odometry.h
+++ b/otto_controller/Core/Inc/control/odometry.h
@@ -1,51 +1,39 @@
 #ifndef ODOMETRY_H
 #define ODOMETRY_H
 
+typedef struct {
+  union {
+    struct {
+      float right;
+      float left;
+    } setpoint;
+    float setpoints[2];
+    struct {
+      float right;
+      float left;
+    } velocity;
+    float velocites[2];
+  };
+
+  float linear_velocity;
+  float angular_velocity;
+  float baseline;
+} Odometry;
+
+void odometry_setpoint_from_cmdvel(Odometry *odom, float linear_vel,
+                                   float angular_vel) {
+  odom->setpoint.left = linear_vel - (odom->baseline * angular_vel) / 2;
+  odom->setpoint.right = linear_vel + (odom->baseline * angular_vel) / 2;
+}
+
+#if 0
 class Odometry {
- private:
-
-  float left_setpoint_;
-  float right_setpoint_;
-  float linear_velocity_;
-  float angular_velocity_;
-  float baseline_;
-
- public:
-  Odometry() {
-    left_setpoint_ = 0;
-    right_setpoint_ = 0;
-    baseline_ = 0;
-  }
-
-  Odometry(float baseline) {
-    left_setpoint_ = 0;
-    right_setpoint_ = 0;
-    baseline_ = baseline;
-  }
-
-  void FromCmdVelToSetpoint(float linear_vel, float angular_vel) {
-    left_setpoint_ = linear_vel - (baseline_ * angular_vel) / 2;
-    right_setpoint_ = linear_vel + (baseline_ * angular_vel) / 2;
-  }
-
+  // NOTE(lb): can i delete this?
   void FromWheelVelToOdom(float left_wheel_vel, float right_wheel_vel){
     linear_velocity_ = (left_wheel_vel + right_wheel_vel)/2;
     angular_velocity_ = (right_wheel_vel - left_wheel_vel)/baseline_;
   }
-
-  float GetLeftVelocity() {
-    return left_setpoint_;
-  }
-  float GetRightVelocity() {
-    return right_setpoint_;
-  }
-  float GetLinearVelocity(){
-    return linear_velocity_;
-  }
-  float GetAngularVelocity(){
-    return angular_velocity_;
-  }
-
 };
+#endif
 
 #endif
diff --git a/otto_controller/Core/Src/main.cpp b/otto_controller/Core/Src/main.cpp
index b646ff6..6dab901 100644
--- a/otto_controller/Core/Src/main.cpp
+++ b/otto_controller/Core/Src/main.cpp
@@ -64,7 +64,7 @@ static Encoder encoders[2] = {{0}, {0}};
 Encoder *encoder_right = &encoders[0];
 Encoder *encoder_left = &encoders[1];
 
-Odometry odom;
+Odometry odom = {0};
 
 //PID
 Pid left_pid;
@@ -176,7 +176,7 @@ int main(void) {
   encoder_right->wheel_circumference = config_msg.right_wheel_circumference;
   encoder_right->ticks_per_revolution = config_msg.ticks_per_revolution;
 
-  odom = Odometry(config_msg.baseline);
+  odom.baseline = config_msg.baseline;
 
   encoder_init(encoders);
   motorcontroller_init(motors);
@@ -313,7 +313,7 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
   motorcontroller_speed_set(motor_right, right_dutycycle);
 }
 
-uint8_t porcoddio = 0;
+uint8_t porcoddio = 0; // NOTE(lb): LOL
 void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) {
   /*
    * Manage received message
@@ -334,10 +334,9 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) {
     otto_status = 3;
   }
 
-  odom.FromCmdVelToSetpoint(linear_velocity, angular_velocity);
-
-  float left_setpoint = odom.GetLeftVelocity();
-  float right_setpoint = odom.GetRightVelocity();
+  odometry_setpoint_from_cmdvel(&odom, linear_velocity, angular_velocity);
+  float left_setpoint = odom.velocity.left;
+  float right_setpoint = odom.velocity.right;
 
   left_pid.Set(left_setpoint);
   right_pid.Set(right_setpoint);