Added CFCs. (#432)

ProjectNeura · Oct 4, 2024 · 139f4cc · 139f4cc
1 parent e3daf5a
commit 139f4cc
Showing 1 changed file with 13 additions and 1 deletion.
diff --git a/leads_vec_rc/cli.py b/leads_vec_rc/cli.py
@@ -18,6 +18,10 @@
 if not exists(config.data_dir):
     L.debug(f"Data directory not found. Creating \"{abspath(config.data_dir)}\"...")
     makedirs(config.data_dir)
+CAR_WIDTH: float = config.get("car_width", 2)
+CAR_LENGTH: float = config.get("car_width", 1)
+CAR_MASS: float = config.get("car_mass", 400)
+CAR_CENTER_OF_MASS: float = config.get("car_center_of_mass", .25)
 
 time_stamp_record: DataPersistence[int] = DataPersistence(2000)
 speed_record: DataPersistence[float] = DataPersistence(2000)
@@ -64,7 +68,15 @@ def on_fail(self, service: Service, error: Exception) -> None:
     def on_receive(self, service: Service, msg: bytes) -> None:
         self.super(service=service, msg=msg)
         try:
-            self.current_data = d = loads(msg.decode())
+            d = loads(msg.decode())
+            mg = CAR_MASS * 2.451675
+            f_forward = CAR_MASS * d["forward_acceleration"] * CAR_CENTER_OF_MASS * .5 / CAR_WIDTH
+            f_lateral = CAR_MASS * d["lateral_acceleration"] * CAR_CENTER_OF_MASS * .5 / CAR_LENGTH
+            d["cfc_fl"] = mg - f_lateral - f_forward
+            d["cfc_fr"] = mg + f_lateral - f_forward
+            d["cfc_rl"] = mg - f_lateral + f_forward
+            d["cfc_rr"] = mg + f_lateral + f_forward
+            self.current_data = d
             acceleration_record.append(Vector(d["forward_acceleration"], d["lateral_acceleration"],
                                               d["vertical_acceleration"]))
             gps_record.append(Vector(d["latitude"], d["longitude"]))