save

Bvngee · Bvngee · commit e19ca9c9cadd · 2025-06-17T15:36:44.000-07:00
diff --git a/Telemetry-Main/BT817Q/layouts.cpp b/Telemetry-Main/BT817Q/layouts.cpp
@@ -37,28 +37,29 @@ void Layouts::drawTestLayout(int var) {
   endFrame(); // automatically waits for cmd queue to clear out
 }
 
-void Layouts::drawStandardLayout(Faults faults, const uint8_t speed, const uint8_t soc,
-                                 uint8_t acc_temp, uint8_t ctrl_tmp,
-                                 uint8_t mtr_tmp, float mtr_volt, float glv,
-                                 float brake_balance, float throttle_demand,
-                                 float brake_demand,
-                                 std::chrono::milliseconds time,
-                                 double delta_time_seconds) {
+void Layouts::drawStandardLayout(
+    Faults faults, const uint8_t speed, const uint8_t soc, uint8_t acc_temp,
+    uint8_t ctrl_tmp, uint8_t mtr_tmp, float mtr_volt, float glv,
+    float steering_angle, float brake_balance, float brake_f, float brake_r,
+    float throttle_demand, float brake_demand, std::chrono::milliseconds time,
+    double delta_time_seconds) {
   if (failure == startFrame()) {
     return;
   }
   loadFonts();
   // printf("Drew!\n");
   clear(255, 255, 255); // black background for frame
   setMainColor(black);
-  drawProgressBar(Point{220, 10}, 340, 35, speed, 110, mid_gray, orange);       // mph progress bar
+  drawProgressBar(Point{220, 10}, 340, 35, speed, 110, mid_gray,
+                  orange);       // mph progress bar
   for (int i = 0; i < 16; i++) { // to section off the progress bar
     uint16_t base_x = 220 + (20 * (i + 1));
     drawLine(Point{base_x, 0}, Point{base_x, 50}, white, 16 * 5);
   }
   drawRect(Point{200, 10}, Point{220, 45}, white);
   drawRect(Point{560, 10}, Point{580, 45}, white);
-  drawProgressBar(Point{220, 180}, 340, 30, soc, 100, mid_gray, Color{0, 200, 36}); // soc progress bar
+  drawProgressBar(Point{220, 180}, 340, 30, soc, 100, mid_gray,
+                  Color{0, 200, 36}); // soc progress bar
   drawRect(Point{200, 180}, Point{220, 210}, white);
   drawRect(Point{560, 180}, Point{580, 210}, white);
   for (int i = 0; i < 16; i++) { // to section off the progress bar
@@ -86,18 +87,25 @@ void Layouts::drawStandardLayout(Faults faults, const uint8_t speed, const uint8
   const auto time_ms = time - duration_cast<milliseconds>(time_s);
   const auto time_min = duration_cast<minutes>(time_s);
   time_s -= duration_cast<seconds>(time_min);
-  drawFormattedText(100, 100, "%02d:%02d:%02d", 24, OPT_CENTER, time_min.count(), time_s.count(), time_ms.count()); // lap time formatted in mm:ss::ms
+  drawFormattedText(100, 100, "%02d:%02d:%02d", 24, OPT_CENTER,
+                    time_min.count(), time_s.count(),
+                    time_ms.count()); // lap time formatted in mm:ss::ms
   const char sign_char = (delta_time_seconds > 0) ? '+' : ' ';
   const Color delta_color = (delta_time_seconds > 0) ? red : green;
-  drawFormattedText(100, 150, "%c %00.2f s  ", delta_color, 24, OPT_CENTER, sign_char, delta_time_seconds); // delta time formatted in ± s.ms
-  //TODO! fix large font on boot up
-  drawFormattedText(400, 110, "%d MPH", 1, OPT_CENTER, speed); // speed display text
+  drawFormattedText(100, 150, "%c %00.2f s  ", delta_color, 24, OPT_CENTER,
+                    sign_char,
+                    delta_time_seconds); // delta time formatted in ± s.ms
+  // TODO! fix large font on boot up
+  drawFormattedText(400, 110, "%d MPH", 1, OPT_CENTER,
+                    speed); // speed display text
   drawFormattedText(400, 160, "SOC: %d", 24, OPT_CENTER, soc);
-  drawFormattedText(100, 260, "BB: %02.1f %%  ", 24, OPT_CENTER, brake_balance * 100.0f); // brake balance display
+  drawFormattedText(100, 260, "BB: %02.1f %%  ", 24, OPT_CENTER,
+                    brake_balance * 100.0f); // brake balance display
   drawFormattedText(680, 80, "ACC: %03d C", 24, OPT_CENTER, acc_temp); // temps
   drawFormattedText(680, 115, "CTRL: %03d C", 24, OPT_CENTER, ctrl_tmp);
   drawFormattedText(680, 150, "MTR: %03d C", 24, OPT_CENTER, mtr_tmp);
-  drawFormattedText(680, 205, "MC: %03.1f V  ", 24, OPT_CENTER, mtr_volt); // voltages
+  drawFormattedText(680, 205, "MC: %03.1f V  ", 24, OPT_CENTER,
+                    mtr_volt); // voltages
   drawFormattedText(680, 235, "GLV: %03.1f V  ", 24, OPT_CENTER, glv);
 
   // drawProgressBar(Point{700, 300},
@@ -123,22 +131,19 @@ void Layouts::drawStandardLayout(Faults faults, const uint8_t speed, const uint8
   endFrame();
 }
 
-void Layouts::drawStandardLayout2(Faults faults, uint8_t speed, uint8_t soc,
-                                  uint8_t acc_temp, uint8_t ctrl_tmp,
-                                  uint8_t mtr_tmp, float mtr_volt, float glv,
-                                  float brake_balance, float throttle_demand,
-                                  float brake_demand,
-                                  std::chrono::milliseconds time,
-                                  double delta_time_seconds, bool rtds,
-                                  uint16_t rpm) {
+void Layouts::drawStandardLayout2(
+    Faults faults, uint8_t speed, uint8_t soc, uint8_t acc_temp,
+    uint8_t ctrl_tmp, uint8_t mtr_tmp, float mtr_volt, float glv,
+    float steering_angle, float brake_balance, float brake_f, float brake_r,
+    float throttle_demand, float brake_demand, std::chrono::milliseconds time,
+    double delta_time_seconds, bool rtds, uint16_t rpm) {
   if (failure == startFrame()) {
     return;
   }
   clear(255, 255, 255); // black background for frame
   loadFonts();
   setMainColor(black);
 
-
   drawFormattedText(400, 100, "%03d", 1, OPT_CENTER, speed);
 
   drawText(185, 235, "TACH", 21);
@@ -160,7 +165,7 @@ void Layouts::drawStandardLayout2(Faults faults, uint8_t speed, uint8_t soc,
     // segmented soc
     uint16_t segTopLeftY = 490 - j * 19;
     uint16_t segBotRightY = segTopLeftY - 10;
-    if (soc < j*4 && !pastPoint) {
+    if (soc < j * 4 && !pastPoint) {
       setMainColor(mid_gray);
       pastPoint = true;
     }
@@ -197,6 +202,9 @@ void Layouts::drawStandardLayout2(Faults faults, uint8_t speed, uint8_t soc,
   drawFormattedText(70, 300, "MC       %03.1fV  ", 24, OPT_CENTER,
                     mtr_volt); // voltages
   drawFormattedText(70, 330, "GLV     %03.1fV  ", 24, OPT_CENTER, glv);
+  drawFormattedText(70, 390, "BRF     %03.2f  ", 24, OPT_CENTER, brake_f);
+  drawFormattedText(70, 420, "BRR     %03.2f  ", 24, OPT_CENTER, brake_r);
+  drawFormattedText(70, 450, "STEER   %03.2f  ", 24, OPT_CENTER, steering_angle);
 
   drawRect(Point{614, 127}, Point{637, 310}, mid_gray);
   uint16_t throttle_bar_h = (310 - floor(183 * throttle_demand));
@@ -205,6 +213,7 @@ void Layouts::drawStandardLayout2(Faults faults, uint8_t speed, uint8_t soc,
   drawRect(Point{643, 127}, Point{666, 310}, mid_gray);
   uint16_t brake_bar_h = (310 - floor(183 * brake_demand));
   drawRect(Point{643, brake_bar_h}, Point{666, 310}, red);
-  // drawRect(Point{643, static_cast<uint16_t>(brake_demand)}, Point{666, 310}, red);
+  // drawRect(Point{643, static_cast<uint16_t>(brake_demand)}, Point{666, 310},
+  // red);
   endFrame();
 }
diff --git a/Telemetry-Main/BT817Q/layouts.h b/Telemetry-Main/BT817Q/layouts.h
@@ -32,7 +32,10 @@ class Layouts : public BT817Q {
     uint8_t mtr_tmp;
     float mtr_volt;
     float glv;
+    float steering_angle;
     float brake_balance;
+    float brake_f;
+    float brake_r;
     float throttle_demand;
     float brake_demand;
     std::chrono::milliseconds time;
@@ -49,7 +52,10 @@ class Layouts : public BT817Q {
                           uint8_t mtr_tmp,
                           float mtr_volt,
                           float glv,
+                          float steering_angle,
                           float brake_balance,
+                          float brake_f,
+                          float brake_r,
                           float throttle_demand,
                           float brake_demand,
                           std::chrono::milliseconds time,
@@ -65,7 +71,10 @@ class Layouts : public BT817Q {
                        params.mtr_tmp,
                        params.mtr_volt,
                        params.glv,
+                       params.steering_angle,
                        params.brake_balance,
+                       params.brake_f,
+                       params.brake_r,
                        params.throttle_demand,
                        params.brake_demand,
                        params.time,
@@ -79,7 +88,10 @@ class Layouts : public BT817Q {
                           uint8_t mtr_tmp,
                           float mtr_volt,
                           float glv,
+                          float steering_angle,
                           float brake_balance,
+                          float brake_f,
+                          float brake_r,
                           float throttle_demand,
                           float brake_demand,
                           std::chrono::milliseconds time,
@@ -97,7 +109,10 @@ class Layouts : public BT817Q {
                        params.mtr_tmp,
                        params.mtr_volt,
                        params.glv,
+                       params.steering_angle,
                        params.brake_balance,
+                       params.brake_f,
+                       params.brake_r,
                        params.throttle_demand,
                        params.brake_demand,
                        params.time,
diff --git a/Telemetry-Main/VehicleStateManager.cpp b/Telemetry-Main/VehicleStateManager.cpp
@@ -3,6 +3,7 @@
 #include <algorithm>
 #include <chrono>
 #include <cstring>
+#include <numeric>
 #include "CANInterface.hpp"
 #include "CANProtocol.hpp"
 #include "mbed.h"
@@ -26,10 +27,22 @@ void VehicleStateManager::update() {
     readSensorValues();
 }
 
+vector<float> brake_f_acc{10};
+vector<float> brake_r_acc{10};
+
 void VehicleStateManager::readSensorValues() {
-    _vehicleState.steering_sensor =  _steering_sensor.read_u16();
-    _vehicleState.brake_sensor_f =  _brake_sensor_f.read_u16();
-    _vehicleState.brake_sensor_r =  _brake_sensor_r.read_u16();
+    _vehicleState.steering_sensor =  _steering_sensor.read() * 3.3f;
+
+    // for (int i = 0; i < 10; i++) {
+    //     brake_f_acc[i] = _brake_sensor_f.read() * 3.3f;
+    //     brake_r_acc[i] = _brake_sensor_r.read() * 3.3f;
+    // }
+    //
+    // _vehicleState.brake_sensor_f = std::accumulate(brake_f_acc.begin(), brake_f_acc.end(), 0.0);
+    // _vehicleState.brake_sensor_r = std::accumulate(brake_r_acc.begin(), brake_r_acc.end(), 0.0);
+
+    _vehicleState.brake_sensor_f =_brake_sensor_f.read() * 3.3f;
+    _vehicleState.brake_sensor_r =_brake_sensor_r.read() * 3.3f;
 }
 
 void VehicleStateManager::processCANMessage() {
diff --git a/Telemetry-Main/VehicleStateManager.hpp b/Telemetry-Main/VehicleStateManager.hpp
@@ -49,9 +49,9 @@ struct VehicleState {
     VDM_YAW_RATE_t vdmYawRate;
 
     // ---- FROM NUCLEO PINS ----
-    uint16_t steering_sensor;
-    uint16_t brake_sensor_f;
-    uint16_t brake_sensor_r;
+    float steering_sensor;
+    float brake_sensor_f;
+    float brake_sensor_r;
     
 };
 
diff --git a/Telemetry-Main/main.cpp b/Telemetry-Main/main.cpp
@@ -10,13 +10,13 @@
 #include "SDBlockDevice.h"
 
 #define ENABLE_RADIO false
-#define ENABLE_SD true
-#define ENABLE_DASH false
+#define ENABLE_SD false
+#define ENABLE_DASH true
 
 // #define TICKS_PER_SECOND 1000
 
 #define SD_UPDATE_HZ 100ms
-#define DASH_UPDATE_HZ 10ms
+#define DASH_UPDATE_HZ 100ms
 #define RADIO_UPDATE_HZ 1ms
 
 // struct TelemetrySystemState {
@@ -64,7 +64,10 @@ Layouts::StandardLayoutParams params = {
   .mtr_tmp = 20,
   .mtr_volt = 110,
   .glv = 12,
+  .steering_angle = 11,
   .brake_balance = 50,
+  .brake_f = 0,
+  .brake_r = 0,
   .throttle_demand = 0,
   .brake_demand = 0,
   .time = chrono::milliseconds(0),
@@ -114,7 +117,10 @@ void update_dash() {
       .mtr_tmp = vsm_state.smeTemp.MOTOR_TEMP,
       .mtr_volt = static_cast<float>(vsm_state.accPower.PACK_VOLTAGE / 100.0),
       .glv = static_cast<float>(vsm_state.pdbPowerA.GLV_VOLTAGE),
-      .brake_balance = static_cast<float>(vsm_state.brake_sensor_f) / (vsm_state.brake_sensor_f + vsm_state.brake_sensor_r),
+      .steering_angle = vsm_state.steering_sensor,
+      .brake_balance = vsm_state.brake_sensor_f / (vsm_state.brake_sensor_f + vsm_state.brake_sensor_r),
+      .brake_f = vsm_state.brake_sensor_f,
+      .brake_r = vsm_state.brake_sensor_r,
       .throttle_demand = static_cast<float>(vsm_state.smeThrottleDemand.TORQUE_DEMAND/32768.0),
       // .throttle_demand = static_cast<float>(vsm_state.etcStatus.PEDAL_TRAVEL),
       .brake_demand = static_cast<float>(((vsm_state.etcStatus.BRAKE_SENSE_VOLTAGE / 1000.0) - 0.5) / 4),
@@ -123,6 +129,7 @@ void update_dash() {
       .rtds = static_cast<bool>(vsm_state.etcStatus.RTDS),
       .rpm = vsm_state.smeTrqSpd.SPEED,
   };
+  printf("%f %f\n", params.brake_f, params.brake_r);
   // params.speed++;
   eve.drawStandardLayout2(params);
 }
@@ -223,6 +230,8 @@ int main() {
     if (ENABLE_DASH) {
         queue.call_every(DASH_UPDATE_HZ, &update_dash);
     }
+    queue.call_every(1ms, [](){vsm.update();});
+    queue.dispatch_forever();
 
     while (true) {
         // t.reset();
