Update 12V via polling

Software/src/battery/FORD-MACH-E-BATTERY.cpp

@@ -1,6 +1,7 @@
 #include "FORD-MACH-E-BATTERY.h"
 #include <Arduino.h>
 #include "../datalayer/datalayer.h"
+#include "../devboard/utils/events.h"
 #include "../devboard/utils/logging.h"
 
 void FordMachEBattery::update_values() {
@@ -9,7 +10,7 @@ void FordMachEBattery::update_values() {
 
   datalayer.battery.status.soh_pptt = battery_soh * 100;
 
-  datalayer.battery.status.voltage_dV;
+  datalayer.battery.status.voltage_dV = battery_voltage * 10;
 
   datalayer.battery.status.current_dA;
 
@@ -17,9 +18,9 @@ void FordMachEBattery::update_values() {
 
   datalayer.battery.status.remaining_capacity_Wh;
 
-  datalayer.battery.status.max_discharge_power_W;
+  datalayer.battery.status.max_discharge_power_W = 5000;  //TODO, fix
 
-  datalayer.battery.status.max_charge_power_W;
+  datalayer.battery.status.max_charge_power_W = 5000;  //TODO, fix
 
   maximum_cellvoltage_mV = datalayer.battery.status.cell_voltages_mV[0];
   minimum_cellvoltage_mV = datalayer.battery.status.cell_voltages_mV[0];
@@ -56,12 +57,18 @@ void FordMachEBattery::update_values() {
   datalayer.battery.status.temperature_min_dC = minimum_temperature * 10;
 
   datalayer.battery.status.temperature_max_dC = maximum_temperature * 10;
+
+  // Check vehicle specific safeties
+  if (polled_12V < 11800) {
+    set_event(EVENT_12V_LOW, 0);
+  }
 }
 
 void FordMachEBattery::handle_incoming_can_frame(CAN_frame rx_frame) {
   switch (rx_frame.ID) {  //These frames are transmitted by the battery
     case 0x07a:           //10ms
       datalayer.battery.status.CAN_battery_still_alive = CAN_STILL_ALIVE;
+      battery_voltage = (((rx_frame.data.u8[2] & 0x03) << 8) | rx_frame.data.u8[3]) / 2;
       break;
     case 0x07b:  //10ms
       datalayer.battery.status.CAN_battery_still_alive = CAN_STILL_ALIVE;
@@ -163,6 +170,26 @@ void FordMachEBattery::handle_incoming_can_frame(CAN_frame rx_frame) {
       break;
     case 0x46f:  //100ms
       datalayer.battery.status.CAN_battery_still_alive = CAN_STILL_ALIVE;
+      break;
+    case 0x7EC:  //OBD2 diag reply from BMS (Replies to both 7DF and 7E4)
+
+      if (rx_frame.data.u8[0] < 0x10) {  //One line response
+        pid_reply = ((rx_frame.data.u8[1] & 0x0F) << 8) | rx_frame.data.u8[2];
+      }
+
+      if (rx_frame.data.u8[0] == 0x10) {  //Multiframe response, send ACK
+        //transmit_can_frame(&FORD_PID_ACK); //Not seen yet
+        //pid_reply = (rx_frame.data.u8[3] << 8) | rx_frame.data.u8[4];
+      }
+
+      switch (pid_reply) {
+        case 0x142:  //12V battery
+          polled_12V = (rx_frame.data.u8[3] << 8) | rx_frame.data.u8[4];
+          break;
+        default:
+          break;
+      }
+
       break;
     default:
       break;
@@ -170,18 +197,32 @@ void FordMachEBattery::handle_incoming_can_frame(CAN_frame rx_frame) {
 }
 
 void FordMachEBattery::transmit_can(unsigned long currentMillis) {
-  // Send 10ms CAN Message
+  // Send 20ms CAN Message
-  if (currentMillis - previousMillis10 >= INTERVAL_10_MS) {
+  if (currentMillis - previousMillis20 >= INTERVAL_20_MS) {
-    previousMillis10 = currentMillis;
+    previousMillis20 = currentMillis;
 
+    if (datalayer.battery.status.bms_status == FAULT) {
+      FORD_25B.data.u8[2] = 0x01;
+    } else {
+      FORD_25B.data.u8[2] = 0x09;
+    }
+
+    transmit_can_frame(&FORD_25B);
+
+    //Full vehicle emulation, not required
+    /*
     //transmit_can_frame(&FORD_217); Not needed for contactor closing
     //transmit_can_frame(&FORD_442); Not needed for contactor closing
+    */
   }
 
   // Send 30ms CAN Message
   if (currentMillis - previousMillis30 >= INTERVAL_30_MS) {
     previousMillis30 = currentMillis;
 
+    //Full vehicle emulation, not required
+    /*
+
     counter_30ms = (counter_30ms + 1) % 16;  // cycles 0-15
 
     // Byte 2: upper nibble = 0xF, lower nibble = (0xF - counter_10ms) % 16
@@ -224,24 +265,29 @@ void FordMachEBattery::transmit_can(unsigned long currentMillis) {
     //transmit_can_frame(&FORD_230); Not needed for contactor closing
     //transmit_can_frame(&FORD_415); Not needed for contactor closing
     //transmit_can_frame(&FORD_4C);  Not needed for contactor closing
-    transmit_can_frame(&FORD_7E);
+    //transmit_can_frame(&FORD_7E); Not needed for contactor closing
-    transmit_can_frame(&FORD_48);
+    //transmit_can_frame(&FORD_48); Not needed for contactor closing
-    transmit_can_frame(&FORD_165);
+    //transmit_can_frame(&FORD_165); Not needed for contactor closing
-    transmit_can_frame(&FORD_7F);
+    //transmit_can_frame(&FORD_7F); Not needed for contactor closing
     transmit_can_frame(&FORD_200);
+    */
   }
   // Send 50ms CAN Message
   if (currentMillis - previousMillis50 >= INTERVAL_50_MS) {
     previousMillis50 = currentMillis;
-    transmit_can_frame(&FORD_42C);
+    //transmit_can_frame(&FORD_42C); Not needed for contactor closing
-    transmit_can_frame(&FORD_42F);
+    //transmit_can_frame(&FORD_42F); Not needed for contactor closing
-    transmit_can_frame(&FORD_43D);
+    //transmit_can_frame(&FORD_43D);
   }
 
   // Send 100ms CAN Message
   if (currentMillis - previousMillis100 >= INTERVAL_100_MS) {
     previousMillis100 = currentMillis;
 
+    transmit_can_frame(&FORD_185);  // Required to close contactors
+
+    //Full vehicle emulation, not required
+    /*
     transmit_can_frame(
         &FORD_12F);  //This message actually has checksum/counter, but it seems to close contactors without those
     transmit_can_frame(&FORD_332);
@@ -254,23 +300,35 @@ void FordMachEBattery::transmit_can(unsigned long currentMillis) {
     transmit_can_frame(&FORD_166);
     transmit_can_frame(&FORD_175);
     transmit_can_frame(&FORD_178);
-    transmit_can_frame(&FORD_203);
+    transmit_can_frame(&FORD_203);  //MANDATORY FOR CONTACTOR OPERATION
     transmit_can_frame(
         &FORD_176);  //This message actually has checksum/counter, but it seems to close contactors without those
-    transmit_can_frame(&FORD_185);
+*/
   }
+
+  // Send 250ms CAN Message
+  if (currentMillis - previousMillis250 >= INTERVAL_250_MS) {
+    previousMillis250 = currentMillis;
+
+    transmit_can_frame(&FORD_PID_REQUEST_7DF);
+  }
+
   // Send 1s CAN Message
   if (currentMillis - previousMillis1000 >= INTERVAL_1_S) {
     previousMillis1000 = currentMillis;
+    //Full vehicle emulation, not required
+    /*
     transmit_can_frame(&FORD_3C3);
     transmit_can_frame(&FORD_581);
+    */
   }
 }
 
 void FordMachEBattery::setup(void) {  // Performs one time setup at startup
   strncpy(datalayer.system.info.battery_protocol, Name, 63);
   datalayer.system.info.battery_protocol[63] = '\0';
-  datalayer.battery.info.number_of_cells = 96;  //TODO, Are all mach-e batteries 96S?
+  datalayer.battery.info.number_of_cells = 96;       //TODO, Are all mach-e batteries 96S?
+  datalayer.battery.info.total_capacity_Wh = 88000;  //Start in 88kWh mode, update later
   datalayer.battery.info.max_cell_voltage_mV = MAX_CELL_VOLTAGE_MV;
   datalayer.battery.info.min_cell_voltage_mV = MIN_CELL_VOLTAGE_MV;
   datalayer.battery.info.max_cell_voltage_deviation_mV = MAX_CELL_DEVIATION_MV;

Software/src/battery/FORD-MACH-E-BATTERY.h

@@ -12,16 +12,17 @@ class FordMachEBattery : public CanBattery {
   static constexpr const char* Name = "Ford Mustang Mach-E battery";
 
  private:
-  static const int MAX_PACK_VOLTAGE_DV = 5000;  //TODO SET
+  static const int MAX_PACK_VOLTAGE_DV = 4140;
   static const int MIN_PACK_VOLTAGE_DV = 2000;  //TODO SET
   static const int MAX_CELL_DEVIATION_MV = 250;
   static const int MAX_CELL_VOLTAGE_MV = 4250;
   static const int MIN_CELL_VOLTAGE_MV = 2900;
 
-  unsigned long previousMillis10 = 0;    // will store last time a 10ms CAN Message was send
+  unsigned long previousMillis20 = 0;    // will store last time a 20ms CAN Message was send
   unsigned long previousMillis30 = 0;    // will store last time a 10ms CAN Message was send
   unsigned long previousMillis50 = 0;    // will store last time a 100ms CAN Message was send
   unsigned long previousMillis100 = 0;   // will store last time a 100ms CAN Message was send
+  unsigned long previousMillis250 = 0;   // will store last time a 100ms CAN Message was send
   unsigned long previousMillis1000 = 0;  // will store last time a 1s CAN Message was send
   unsigned long previousMillis10s = 0;   // will store last time a 10s CAN Message was send
 
@@ -30,12 +31,40 @@ class FordMachEBattery : public CanBattery {
   int16_t minimum_temperature = 0;
   uint16_t battery_soc = 5000;
   uint16_t battery_soh = 99;
+  uint16_t battery_voltage = 370;
   uint16_t maximum_cellvoltage_mV = 3700;
   uint16_t minimum_cellvoltage_mV = 3700;
 
   uint8_t counter_30ms = 0;
   uint8_t counter_8_30ms = 0;
+  uint16_t pid_reply = 0;
 
+  uint16_t polled_12V = 12000;
+
+  CAN_frame FORD_PID_REQUEST_7DF = {.FD = false,
+                                    .ext_ID = false,
+                                    .DLC = 8,
+                                    .ID = 0x7DF,
+                                    .data = {0x02, 0x01, 0x42, 0x00, 0x00, 0x00, 0x00, 0x00}};
+  CAN_frame FORD_PID_ACK = {.FD = false,
+                            .ext_ID = false,
+                            .DLC = 8,
+                            .ID = 0x7DF,
+                            .data = {0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+  //Message needed for contactor closing
+  CAN_frame FORD_25B = {.FD = false,
+                        .ext_ID = false,
+                        .DLC = 8,
+                        .ID = 0x25B,
+                        .data = {0x01, 0xF4, 0x09, 0xF4, 0xE0, 0x00, 0x80, 0x00}};
+  CAN_frame FORD_185 = {.FD = false,
+                        .ext_ID = false,
+                        .DLC = 8,
+                        .ID = 0x185,
+                        .data = {0x03, 0x4E, 0x75, 0x32, 0x00, 0x80, 0x00, 0x00}};
+  //Messages to emulate full vehicle
+  /*
   CAN_frame FORD_47 = {.FD = false,
                        .ext_ID = false,
                        .DLC = 8,
@@ -116,11 +145,7 @@ class FordMachEBattery : public CanBattery {
                         .DLC = 8,
                         .ID = 0x12F,
                         .data = {0x0A, 0xF8, 0x3F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}};
-  CAN_frame FORD_185 = {.FD = false,
+
-                        .ext_ID = false,
-                        .DLC = 8,
-                        .ID = 0x185,
-                        .data = {0x03, 0x4E, 0x75, 0x32, 0x00, 0x00, 0x00, 0x00}};
   CAN_frame FORD_200 = {.FD = false,
                         .ext_ID = false,
                         .DLC = 8,
@@ -146,11 +171,7 @@ class FordMachEBattery : public CanBattery {
                         .DLC = 8,
                         .ID = 0x230,
                         .data = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03}};
-  CAN_frame FORD_25B = {.FD = false,
+
-                        .ext_ID = false,
-                        .DLC = 8,
-                        .ID = 0x25B,
-                        .data = {0x01, 0xF4, 0x09, 0xF4, 0xE0, 0x00, 0x80, 0x00}};
   CAN_frame FORD_2EC = {.FD = false,
                         .ext_ID = false,
                         .DLC = 8,
@@ -216,6 +237,7 @@ class FordMachEBattery : public CanBattery {
                         .DLC = 8,
                         .ID = 0x4B0,
                         .data = {0x81, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}};
+                        */
 };
 
 #endif