save

Author: Bvngee

Telemetry-Main/BT817Q/layouts.cpp

@@ -205,5 +205,6 @@ 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);
   endFrame();
 }

Telemetry-Main/VehicleStateManager.cpp

@@ -7,8 +7,12 @@
 #include "CANProtocol.hpp"
 #include "mbed.h"
 
-VehicleStateManager::VehicleStateManager(MbedCAN* mbedCAN)
-    : _mbedCAN(mbedCAN)
+VehicleStateManager::VehicleStateManager(
+    MbedCAN* mbedCAN,
+    PinName steering_sensor,
+    PinName brake_sensor_f,
+    PinName brake_sensor_r
+) : _mbedCAN(mbedCAN), _steering_sensor(steering_sensor), _brake_sensor_f(brake_sensor_r), _brake_sensor_r(brake_sensor_r)
 {
     _vehicleState = {};
 
@@ -19,16 +23,19 @@ void VehicleStateManager::update() {
     // printf("Updating CAN\n");
     processCANMessage();
     updateLapTime();
+    readSensorValues();
+}
+
+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();
 }
 
 void VehicleStateManager::processCANMessage() {
     if (!_mbedCAN || !_mbedCAN->isReady()) { return; }
-    // printf("Here\n");
     CANMessage msg;
-    // int processed = 0;
-    while ((_mbedCAN->read(msg))) {
-        // printf("%x\n", msg.id);
-        // processed++;
+    while (_mbedCAN->read(msg)) {
         switch (msg.id) {
             // ACC Message
             case CAN_ID::ACC_STATUS: {

Telemetry-Main/VehicleStateManager.hpp

@@ -20,6 +20,7 @@
 #endif
 
 struct VehicleState {
+    // ---- FROM CAN BUS ----
     // ACC
     ACC_STATUS_t accStatus;
     ACC_SEG_VOLTS_t accSegVolts[5];
@@ -46,12 +47,23 @@ struct VehicleState {
     VDM_DATE_TIME_t vdmDateTime;
     VDM_ACCELERATION_t vdmAcceleration;
     VDM_YAW_RATE_t vdmYawRate;
+
+    // ---- FROM NUCLEO PINS ----
+    uint16_t steering_sensor;
+    uint16_t brake_sensor_f;
+    uint16_t brake_sensor_r;
+    
 };
 
 class VehicleStateManager {
 
 public:
-    explicit VehicleStateManager(MbedCAN* mbedCAN);
+    explicit VehicleStateManager(
+        MbedCAN* mbedCAN,
+        PinName steering_sensor,
+        PinName brake_sensor_f,
+        PinName brake_sensor_r
+    );
 
     VehicleState getState() const;
     void update();
@@ -60,13 +72,18 @@ class VehicleStateManager {
 
 private:
     MbedCAN* _mbedCAN;
+    AnalogIn _steering_sensor;
+    AnalogIn _brake_sensor_f;
+    AnalogIn _brake_sensor_r;
 
     VehicleState _vehicleState;
+
     char _lapTime[16];
     Timer _lapTimer;
 
     void processCANMessage();
     void updateLapTime();
+    void readSensorValues();
 };
 
-#endif // VEHICLE_STATE_MANAGER_HPP 
+#endif // VEHICLE_STATE_MANAGER_HPP 

Telemetry-Main/main.cpp

@@ -34,7 +34,7 @@ SPI spi(PA_7, PA_6, PA_5);
 XBeeRadio radio(spi, cs, spi_attn);
 auto mbed_can = CAN(PB_8, PB_9, 500000);
 auto can = MbedCAN(mbed_can);
-auto vsm = VehicleStateManager(&can);
+auto vsm = VehicleStateManager(&can, PC_5, PC_1, PC_0);
 
 // Default DASH parameters
 Layouts::StandardLayoutParams params = {
@@ -69,39 +69,33 @@ void update_sd() {
   // printf("SD!\n");
 }
 void update_dash() {
-  // printf("DASH!\n");
   const VehicleState vsm_state = vsm.getState();
-  int8_t total_temp = 0;
+  // int8_t total_temp = 0;
   uint8_t max_temp = 0;
   for (auto [TEMPS_CELL0, TEMPS_CELL1, TEMPS_CELL2, TEMPS_CELL3, TEMPS_CELL4,TEMPS_CELL5] : vsm_state.accSegTemps) {
-    total_temp += (TEMPS_CELL0 + TEMPS_CELL1 + TEMPS_CELL2 + TEMPS_CELL3 + TEMPS_CELL4 + TEMPS_CELL5);
+    // total_temp += (TEMPS_CELL0 + TEMPS_CELL1 + TEMPS_CELL2 + TEMPS_CELL3 + TEMPS_CELL4 + TEMPS_CELL5);
     max_temp = max( TEMPS_CELL0, max(TEMPS_CELL1, max(TEMPS_CELL2, max(TEMPS_CELL3, max(TEMPS_CELL4, TEMPS_CELL5)))));
   }
   params = {
       .faults =
           Faults{false, static_cast<bool>(vsm_state.accStatus.PRECHARGE_DONE),
                  static_cast<bool>(vsm_state.accStatus.SHUTDOWN_STATE)},
-      // .speed = static_cast<uint8_t>(vsm_state.vdmGpsData.SPEED / 100),
-      .speed = static_cast<uint8_t>(vsm_state.etcStatus.PEDAL_TRAVEL),
+      .speed = static_cast<uint8_t>(vsm_state.smeThrottleDemand.TORQUE_DEMAND / 327.68),
       .soc = vsm_state.accPower.SOC,
       .acc_temp = max_temp,
       .ctrl_tmp = vsm_state.smeTemp.CONTROLLER_TEMP,
       .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 = 50,
-      // .throttle_demand =
-      // static_cast<float>(static_cast<float>(vsm_state.smeThrottleDemand.TORQUE_DEMAND)/30000.0),
-      .throttle_demand = static_cast<float>(vsm_state.etcStatus.PEDAL_TRAVEL),
-      .brake_demand = static_cast<float>(
-          ((static_cast<float>(vsm_state.etcStatus.BRAKE_SENSE_VOLTAGE) /
-            1000.0) -
-           0.5) /
-          4),
+      .brake_balance = static_cast<float>(vsm_state.brake_sensor_f) / (vsm_state.brake_sensor_f + 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),
       .time = chrono::milliseconds(0),
       .delta_time_seconds = 0.01,
-      .rtds = false,
-      .rpm = vsm_state.smeTrqSpd.SPEED};
+      .rtds = static_cast<bool>(vsm_state.etcStatus.RTDS),
+      .rpm = vsm_state.smeTrqSpd.SPEED,
+  };
   // params.speed++;
   eve.drawStandardLayout2(params);
 }
@@ -153,9 +147,6 @@ int main() {
         }
 
         x++;
-        // Remember to read f and r brake pressure
-
-
         // if (state.radio_event) {
         //     state.radio_event = false;
         //     if (state.radio_on) {
@@ -175,14 +166,12 @@ int main() {
         //     update_dash();
         //     state.dash_event = false;
         // }
-        // printf("Time: %f\n", t.elapsed_time().count()/1.0);
-        // t.reset();
-        // update_dash();
-        // printf("\tDash: %f\n", t.elapsed_time().count()/1.0);
-        if (x>7000) {
-            // printf("Time: %f\n", t.elapsed_time().count()/1.0);
+        if (x>5000) {
+            printf("Time: %f\n", t.elapsed_time().count()/1.0);
             t.reset();
+
             update_dash();
+
             // eve.drawStandardLayout2(p);
             printf("\tDash: %.3fms\n", t.elapsed_time().count()/1000.0);