CAN-Gateway für lenkwinkelabhängige Einparkhilfslinien im vFL nachrüsten

Das System mit Rückfahrkamera fand erst im FL serienmäßig Einzug, in Kombination mit dem Navigationssystem MCA-Plus. Im vFL war es nur in den letzten Modellen kurz vor dem Wechsel zum FL verfügbar, dort noch mit dem NX-Plus.

Grundsätzlich funktioniert eine Nachrüstung auch im vFL, jedoch würden ohne weitere Vorkehrung die Einpark-Hilfslinien im Videobild nicht lenkwinkelabhängig gebogen, sie blieben einfach gerade. Grund dafür ist, das für die Krümmung der Lenkwinkel, also die Stellung des Lenkrades benötigt wird, welche beim vFL nicht auf dem MS-CAN Bus gesendet wird. Dies übernimmt im FL das BCM, ist also Softwareabhängig. Die Software des FL-BCM lässt sich aber nicht auf die eines vFL-BCM aufspielen, da die Hardware unterschiedlich ist.

Das Rückfahrkamera-Modul erhält im FL den Lenkwinkel über die CAN ID 480 auf dem MS-CAN Bus. In dieser Botschaft ist in den letzten beiden Bytes (D0 und D1, wenn man von links nach rechts die Bytes von D7 bis D0 bezeichnet) der Lenkwinkel kodiert. Im vFL findet sich auf dem MS-CAN Bus keine Lenkwinkelinformation, daher muss man diese vom HS-CAN Bus dorthin übertragen. Zum Glück sendet das vFL BCM auf der ID 480 keine Botschaften, man kann diese also einfach dort einspeisen ohne Sorge eines Konfliktes. Die Kodierung des Lenkwinkels muss dann noch von HS-CAN auf MS-CAN angepasst (umgerechnet) werden und schon hat man mitlenkende Hilfslinien im Videobild!

Ziel der Emulation ist das erzeugen der original CAN-Botschaft mit der ID 0x480. Das Sendeintervall beträgt 200 ms. Die Datenlänge (DLC) 8 Bytes, von denen das Rückfahrkameramodul jedoch nur die Bytes D0 + D1 auswertet. Die CAN-Botschaft hat diesen Aufbau: 480 8 * * * * * * LW-MSB LW-LSB.

Der durchfahrbare Winkel des Lenkrades ist -450,0° bis +450,0°. In Mittelstellung des Lenkrades sollte der Lenkwinkel 0° betragen. Dreht man des Lenkrad von dieser Position aus gegen den Uhrzeigersinn (links herum) wird der Wert negativ, dreht man ihn im Uhrzeigersinn (rechts herum) positiv.

Der Drehwinkel wird in den beiden Bytes als 2-Byte Fließkommawert (SIGNED WORD) mit 1 Nachkommastelle (Big-Endian) kodiert. Dabei fungiert das oberste Bit von D1 als Sign-Flag, sprich 0b1xxx xxxx xxxx xxxx signalisiert einen negativen Wert, 0b0xxx xxxx xxxx xxxx einen positiven. Der Wert durch 10 ergibt den Winkel in Grad und umgekehrt der Winkel mal 10 den Wert des Bytes. In Mittelstellung ist der Wert 0x0000, bei Vollanschlag links ca. 0xAC63 (0b 1010 1100 0110 0011), bei Vollanschlag rechts ca. 0x2C57 (0b 0010 1100 0101 0111).

Der Lenkwinkel auf dem HS-CAN ist in der Botschaft ID 0x076 enthalten und wird alle 10 Millisekunden vom SCCM (Steering-Column-Control-Module) gesendet. Hier wird der Winkelwert in den letzten beiden Bytes D1 und D0 kodiert und die Drehrichtung im obersten (D7). In Mittelstellung (bzw. bei Zündung aus) ist der Winkel-Wert 0x8000. Dreht man das Lenkrad steigt der Wert bis zum Anschlag auf ca. 0xAC20. Dreht man im Uhrzeigersinn (rechts herum) ist der Wert von D7 < 0x40, dreht man gegen den Uhrzeigersinn (links herum) ist der >= 0x40.

In Pseudo-Code ausgedrückt sollte die Software wie folgt arbeiten:

1. Init CAN-Shield on HS-CAN
2. Init CAN-Shield on MS-CAN
3. Read message ID 0x076 from HS-CAN
   1. Set angle value from D1,D0 (Big Endian)
   2. Is Byte D7 < 0x40 then subtract 0x8000 from angle value (steering wheel turned right clockwise from center position)
   3. Send message on MS-CAN

Für die Hardware verwendet man;

1. 1x Arduino Nano
2. 2x CAN-Bus Shield mit MCP2515
3. 1x Spannungsregler (Buck-Regler, oder Automotive LDO-Regler)
4. 1x Gehäuse

Hardwareaufbau:

1. SPI-Chipselect vom CAN-Shield für den HS-CAN (can0) an Pin 10 vom Arduino verbinden
2. SPI-Chipselect vom CAN-Shield für den MS-CAN (can1) an Pin 9 vom Arduino verbinden
3. /INT-Pin vom HS-CAN Shield (can0) an Pin 2 des Arduino verbinden

Die nachfolgend benötigten Libraries herunterladen, entpacken und den Inhalt in den Ordner %USERPROFILE%\Documents\Arduino\libraries kopieren:

1. SPI
2. TimerOne
3. MCP_CAN

Die Software (INO):

/**
 * CAN-Gateway (HS- to MS-CAN) to enable bending of parking-lines in rear view camera module on preFacelift cars (2007-08/2010)
 *
 * (C)2018 by TychoGold/Go4IT
 * Version: 1.0
 * Release: 28.08.2018 20:39
 */
 
#include <SPI.h>
#include <mcp_can.h>
#include <TimerOne.h>
#include <avr/sleep.h>
#include <avr/wdt.h>
 
volatile long unsigned int rxId;  // ID of received CAN message
volatile unsigned char len = 0;   // DLC of received CAN message
volatile unsigned char rxBuf[8];  // 8 Byte receive-buffer for CAN message
 
// Interrupt pins
#define HSCAN_INT_PIN 2
 
// CAN-Shield CS pins
MCP_CAN HSCAN(10);
MCP_CAN MSCAN(9);
 
void setup()
{
  /**
   * Init HS-CAN shield (can0) to read only ID 076 on HS-CAN
   */
  HSCAN.begin(MCP_STDEXT, CAN_500KBPS, MCP_8MHZ);  // use "MCP_16MHZ" if CAN-Shield is supplied with an 16 MHz quarz
  HSCAN.setMode(MODE_CONFIG);
  HSCAN.init_Mask(0,0,0x07FF0000);
  HSCAN.init_Filt(0,0,0x00760000);
  HSCAN.init_Mask(1,0,0x07FF0000);
  HSCAN.init_Filt(2,0,0x00760000);
  HSCAN.setMode(MCP_LISTENONLY);
 
  /**
   * Init MS-CAN shield (can1) to write on MS-CAN
   */
  MSCAN.begin(MCP_STDEXT, CAN_125KBPS, MCP_8MHZ);  // use "MCP_16MHZ" if CAN-Shield is supplied with an 16 MHz quarz
  MSCAN.setMode(MCP_NORMAL);
 
  // Init write timer, to send CAN-ID 0x480 every 150 ms
  Timer1.initialize(150000);
  Timer1.attachInterrupt(mscan_int);
 
  // Init /INT pin from HS-CAN shield
  pinMode(digitalPinToInterrupt(HSCAN_INT_PIN), INPUT_PULLUP);
  attachInterrupt (digitalPinToInterrupt(HSCAN_INT_PIN), hscan_int, LOW);  // if /INT of shield goes low, read message from HS-CAN (also wakeup if on sleep)
 
  // Enable watchdog to restart module after 8 seconds if WD timer was not reset properly from serverloop.
  wdt_enable(WDTO_8S);
}
 
/**
 * ISR for incomming message ID 0x076 on HS-CAN
 */
void hscan_int()
{
  wdt_enable(WDTO_8S);
  wdt_reset();
  HSCAN.readMsgBuf(&rxId, &len, rxBuf);
  set_sleep_mode(SLEEP_MODE_IDLE);
}
 
/**
 * ISR for sending message ID 0x480 on MS-CAN
 */
void mscan_int()
{
  wdt_reset();
 
  // Detect direction of turn
  if (rxBuf[0] >> 4 == 0) {
      rxBuf[6] = rxBuf[6] - 0x80;  // if we turn clockwise, the resulting value for MS-CAN should be positive, so remove Bit 7 of MSB of value
  }
 
  // The CMR-module doesn't like all bytes to be 0x00, so fill "unused" bytes with arbitrary value
  rxBuf[0] = 0xAB;
  rxBuf[1] = 0xAB;
  rxBuf[2] = 0xAB;
  rxBuf[3] = 0xAB;
  rxBuf[4] = 0xAB;
  rxBuf[5] = 0xAB;
 
  // Send modified message as ID 0x480 on MS-CAN
  MSCAN.sendMsgBuf(0x480, 0, 8, rxBuf);
 
  set_sleep_mode(SLEEP_MODE_PWR_DOWN);
  wdt_disable();
}
 
/**
 * MAIN
 */
void loop()
{
  sleep_enable();
  sleep_mode();
  sleep_disable();
}