Merge pull request #1494 from jonny5532/fix/change-can-speed

Fix multiple bugs with native change_can_speed operation to fix BMW PHEV

Software/src/battery/BMW-PHEV-BATTERY.cpp

@@ -177,12 +177,15 @@ void BmwPhevBattery::wake_battery_via_canbus() {
   // Followed by a Recessive interval of at least ~3 µs (min) and at most ~10 µs (max)
   // Then a second dominant pulse of similar timing.
 
-  auto original_speed = change_can_speed(CAN_Speed::CAN_SPEED_100KBPS);
+  change_can_speed(CAN_Speed::CAN_SPEED_100KBPS);
 
   transmit_can_frame(&BMW_PHEV_BUS_WAKEUP_REQUEST);
   transmit_can_frame(&BMW_PHEV_BUS_WAKEUP_REQUEST);
 
-  change_can_speed(original_speed);
+  // FIXME: This might not wait for the above frames to send before it changes
+  // the speed back. A state-machine controlled delay is likely necessary.
+
+  reset_can_speed();
 
   logging.println("Sent magic wakeup packet to SME at 100kbps...");
 }

Software/src/battery/CanBattery.cpp

@@ -1,17 +1,18 @@
 #include "CanBattery.h"
 
-CanBattery::CanBattery(CAN_Speed speed) {
-  can_interface = can_config.battery;
-  register_transmitter(this);
-  register_can_receiver(this, can_interface, speed);
-}
+CanBattery::CanBattery(CAN_Speed speed) : CanBattery(can_config.battery, speed) {}
 
 CanBattery::CanBattery(CAN_Interface interface, CAN_Speed speed) {
   can_interface = interface;
+  initial_speed = speed;
   register_transmitter(this);
   register_can_receiver(this, can_interface, speed);
 }
 
-CAN_Speed CanBattery::change_can_speed(CAN_Speed speed) {
+bool CanBattery::change_can_speed(CAN_Speed speed) {
   return ::change_can_speed(can_interface, speed);
 }
+
+void CanBattery::reset_can_speed() {
+  ::change_can_speed(can_interface, initial_speed);
+}

Software/src/battery/CanBattery.h

@@ -22,11 +22,13 @@ class CanBattery : public Battery, Transmitter, CanReceiver {
 
  protected:
   CAN_Interface can_interface;
+  CAN_Speed initial_speed;
 
   CanBattery(CAN_Speed speed = CAN_Speed::CAN_SPEED_500KBPS);
   CanBattery(CAN_Interface interface, CAN_Speed speed = CAN_Speed::CAN_SPEED_500KBPS);
 
-  CAN_Speed change_can_speed(CAN_Speed speed);
+  bool change_can_speed(CAN_Speed speed);
+  void reset_can_speed();
 
   void transmit_can_frame(const CAN_frame* frame) { transmit_can_frame_to_interface(frame, can_interface); }
 };

Software/src/communication/can/comm_can.cpp

@@ -1,6 +1,4 @@
 #include "comm_can.h"
-#include <algorithm>
-#include <map>
 #include "../../lib/pierremolinaro-ACAN2517FD/ACAN2517FD.h"
 #include "../../lib/pierremolinaro-acan-esp32/ACAN_ESP32.h"
 #include "../../lib/pierremolinaro-acan2515/ACAN2515.h"
@@ -11,6 +9,11 @@
 #include "src/devboard/sdcard/sdcard.h"
 #include "src/devboard/utils/logging.h"
 
+#include <esp_private/periph_ctrl.h>
+
+#include <algorithm>
+#include <map>
+
 volatile CAN_Configuration can_config = {.battery = CAN_NATIVE,
                                          .inverter = CAN_NATIVE,
                                          .battery_double = CAN_ADDON_MCP2515,
@@ -35,7 +38,9 @@ void register_can_receiver(CanReceiver* receiver, CAN_Interface interface, CAN_S
   DEBUG_PRINTF("CAN receiver registered, total: %d\n", can_receivers.size());
 }
 
-ACAN_ESP32_Settings* settingsespcan;
+uint32_t init_native_can(CAN_Speed speed, gpio_num_t tx_pin, gpio_num_t rx_pin);
+
+ACAN_ESP32_Settings* settingsespcan = nullptr;
 
 static uint32_t QUARTZ_FREQUENCY;
 SPIClass SPI2515;
@@ -90,12 +95,7 @@ bool init_CAN() {
       return false;
     }
 
-    settingsespcan = new ACAN_ESP32_Settings((int)nativeIt->second.speed * 1000UL);
-    settingsespcan->mRequestedCANMode = ACAN_ESP32_Settings::NormalMode;
-    settingsespcan->mTxPin = tx_pin;
-    settingsespcan->mRxPin = rx_pin;
-
-    const uint32_t errorCode = ACAN_ESP32::can.begin(*settingsespcan);
+    const uint32_t errorCode = init_native_can(nativeIt->second.speed, tx_pin, rx_pin);
     if (errorCode == 0) {
       native_can_initialized = true;
       logging.println("Native Can ok");
@@ -482,11 +482,41 @@ void restart_can() {
   }
 }
 
-CAN_Speed change_can_speed(CAN_Interface interface, CAN_Speed speed) {
-  auto oldSpeed = (CAN_Speed)settingsespcan->mDesiredBitRate;
-  if (interface == CAN_Interface::CAN_NATIVE) {
-    settingsespcan->mDesiredBitRate = (int)speed;
-    ACAN_ESP32::can.begin(*settingsespcan);
+// Initialize the native CAN interface with the given speed and pins.
+// This can be called repeatedly to change the interface speed (as some
+// batteries require).
+uint32_t init_native_can(CAN_Speed speed, gpio_num_t tx_pin, gpio_num_t rx_pin) {
+
+  // TODO: check whether this is necessary? It seems to help with
+  // reinitialization.
+  periph_module_reset(PERIPH_TWAI_MODULE);
+
+  if (settingsespcan != nullptr) {
+    delete settingsespcan;
   }
-  return speed;
+
+  // Create a new settings object (as it does the bitrate calcs in the constructor)
+  settingsespcan = new ACAN_ESP32_Settings((int)speed * 1000UL);
+  settingsespcan->mRequestedCANMode = ACAN_ESP32_Settings::NormalMode;
+  settingsespcan->mTxPin = tx_pin;
+  settingsespcan->mRxPin = rx_pin;
+
+  // (Re)start the CAN interface
+  return ACAN_ESP32::can.begin(*settingsespcan);
+}
+
+// Change the speed of the given CAN interface. Returns true if successful.
+bool change_can_speed(CAN_Interface interface, CAN_Speed speed) {
+  if (interface == CAN_Interface::CAN_NATIVE && settingsespcan != nullptr) {
+    // Reinitialize the native CAN interface with the new speed
+    const uint32_t errorCode = init_native_can(speed, settingsespcan->mTxPin, settingsespcan->mRxPin);
+    if (errorCode != 0) {
+      logging.print("Error Native Can: 0x");
+      logging.println(errorCode, HEX);
+      return false;
+    }
+    return true;
+  }
+
+  return false;
 }

Software/src/communication/can/comm_can.h

@@ -102,7 +102,7 @@ void stop_can();
 // Restart CAN communication for all interfaces
 void restart_can();
 
-// Change the speed of the CAN interface and return the old speed.
-CAN_Speed change_can_speed(CAN_Interface interface, CAN_Speed speed);
+// Change the speed of the CAN interface. Returns true if successful.
+bool change_can_speed(CAN_Interface interface, CAN_Speed speed);
 
 #endif

test/emul/can.cpp

@@ -5,8 +5,8 @@ void transmit_can_frame_to_interface(const CAN_frame* tx_frame, int interface) {
 
 void register_can_receiver(CanReceiver* receiver, CAN_Interface interface, CAN_Speed speed) {}
 
-CAN_Speed change_can_speed(CAN_Interface interface, CAN_Speed speed) {
-  return CAN_Speed::CAN_SPEED_500KBPS;
+bool change_can_speed(CAN_Interface interface, CAN_Speed speed) {
+  return true;
 }
 
 void stop_can() {}