openinverter forum

Code: Select all

#include <Arduino.h>
#include "driver/twai.h"

// Pins für ESP32 & SN65HVD230
#define TX_PIN GPIO_NUM_16
#define RX_PIN GPIO_NUM_17

// Gemeinsam genutzte Variablen
volatile uint16_t TorqueVal = 10000;
volatile bool motorRunning = false;
volatile unsigned long startTime = 0;
const uint16_t TorqueNeutral = 10000;

//#############################################################
// SETTINGS
int TorqMaxRange = 33;
const unsigned long RunTimeMax_ms = 4000; 
// SETTINGS
//#############################################################


byte motorFunction = 0x03;
int lastSecondsPrint = -1;

volatile int16_t currentRPM = 0;
float currentVoltage = 0;
volatile int16_t maxRPM = 0;
volatile float currentPhase1 = 0;
volatile float currentPhase2 = 0;
volatile float maxPhase1 = 0;
volatile float maxPhase2 = 0;

// TWAI Konfiguration
twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(TX_PIN, RX_PIN, TWAI_MODE_NORMAL);
twai_timing_config_t t_config = TWAI_TIMING_CONFIG_500KBITS();
twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();

// Task-Handles
TaskHandle_t TaskCanSendHandle;
TaskHandle_t TaskCanRecvHandle;
TaskHandle_t TaskSerialHandle;

void sendCAN(uint32_t id, uint8_t len, uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3, uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7)
{
    twai_message_t msg;
    msg.identifier = id;
    msg.extd = 0;
    msg.rtr = 0;
    msg.data_length_code = len;
    msg.data[0] = d0; msg.data[1] = d1; msg.data[2] = d2; msg.data[3] = d3;
    msg.data[4] = d4; msg.data[5] = d5; msg.data[6] = d6; msg.data[7] = d7;
    twai_transmit(&msg, pdMS_TO_TICKS(10));
}

// --- TASK 1: CAN SENDEN & LOGGING (CORE 1) ---
void TaskCanSendLoop(void *pvParameters)
{
    unsigned long lastSendTime = 0;
    for (;;)
    {
        unsigned long currentTime = millis();
        unsigned long elapsedTime = currentTime - startTime;

        if (motorRunning)
        {
            if (elapsedTime >= RunTimeMax_ms)
            {
                motorRunning = false;
                TorqueVal = TorqueNeutral;
                lastSecondsPrint = -1;
                Serial.print("Core 1: Motor Stopp.");
                Serial.print(" | Maximal RPM: ");
                Serial.print(maxRPM);
                Serial.print(" | Max Ph1: "); Serial.print(maxPhase1, 1); Serial.print(" A");
                Serial.print(" | Max Ph2: "); Serial.print(maxPhase2, 1); Serial.println(" A");
    
    // Werte für den nächsten Lauf zurücksetzen
    maxRPM = 0; 
    maxPhase1 = 0;
    maxPhase2 = 0;


                
            }
            else
            {
                float remainingSeconds = (RunTimeMax_ms - elapsedTime + 499) / 1000;
                if (remainingSeconds != lastSecondsPrint && remainingSeconds > 0)
                {
                    if (TorqueVal > 10000) Serial.print("-> Motor rueckwaerts ");
                    else Serial.print("-> Motor vorwaerts ");
                    
                    Serial.print(", Rest: ");
                    Serial.print(remainingSeconds);
                    Serial.print("Sek.");
                    Serial.print(" | Aktuelle RPM: ");
                    Serial.print(currentRPM);
                    Serial.print(" | VCB: ");
                    Serial.print(currentVoltage, 1);
                    Serial.print(" V");
                    Serial.print(" | Ph1: ");
                    Serial.print(currentPhase1,1);
                    Serial.print(" A | Ph2: ");
                    Serial.print(currentPhase2,1);
                    Serial.println(" A");

                    lastSecondsPrint = remainingSeconds;
                }
            }
        }

        // Senden alle 100ms (Timing aus deinem Code)
        if (currentTime - lastSendTime >= 50)
        {
            lastSendTime = currentTime;
            uint8_t tHi = highByte(TorqueVal);
            uint8_t tLo = lowByte(TorqueVal);
            byte currentFunc = motorRunning ? motorFunction : 0x00;

            sendCAN(0x371, 8, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00);
            sendCAN(0x286, 8, 0x00, 0x00, 0x00, 0x3D, 0x00, 0x00, 0x21, 0x00);
            sendCAN(0x285, 8, 0x00, 0x00, 0x14, 0x39, 0x91, 0xFE, 0x0C, 0x10);
            sendCAN(0x287, 8, tHi, tLo, tHi, tLo, tHi, tLo, currentFunc, 0x00);
        }
        vTaskDelay(pdMS_TO_TICKS(10)); 
    }
}

// --- TASK 2: SCHNELLER CAN EMPFANG (CORE 0, PRIO 5) ---
void TaskCanRecvLoop(void *pvParameters)
{
    twai_message_t rx_msg;
    for (;;)
    {
        // Wir nutzen die High-Speed Schleife aus deinem Code
        while (twai_receive(&rx_msg, pdMS_TO_TICKS(1)) == ESP_OK)
        {
          if (rx_msg.identifier == 0x289) 
{
    // Byte 2 ist MSB, Byte 3 ist LSB
    uint16_t rawRpm = (rx_msg.data[2] << 8) | rx_msg.data[3];
    currentRPM = rawRpm - 20000;
    
    // abs() nutzen wir nur zum Vergleich, gespeichert wird die echte Zahl
    if (abs(currentRPM) > abs(maxRPM)) 
    {
        maxRPM = currentRPM; 
    }

    // VCB: Byte 4 ist MSB, Byte 5 ist LSB
    uint16_t rawVoltage = (rx_msg.data[4] << 8) | rx_msg.data[5];
    // Da dein Log 3420 zeigt, teilen wir durch 10.0f für 342.0V
    currentVoltage = rawVoltage / 10.0f; 
}
else if (rx_msg.identifier == 0x732)
{
    // Phase 1: Byte 0 ist MSB, Byte 1 ist LSB
    uint16_t rawPh1 = (rx_msg.data[0] << 8) | rx_msg.data[1];
    // Phase 2: Byte 2 ist MSB, Byte 3 ist LSB
    uint16_t rawPh2 = (rx_msg.data[2] << 8) | rx_msg.data[3];

    // Offset laut DBC ist -1000. 
    // Wir nehmen den Rohwert, ziehen 1000 ab und teilen durch 10.0f, 
    // um auf die 5A aus SavvyCAN zu kommen (Dezi-Ampere).
    currentPhase1 = (float)(rawPh1 - 1000) ;
    currentPhase2 = (float)(rawPh2 - 1000) ;
   // Vergleich über den Betrag (abs), aber Speichern des echten Werts (inkl. Vorzeichen)
    if (abs(currentPhase1) > abs(maxPhase1)) {
        
         maxPhase1 = currentPhase1; 
    }
    if (abs(currentPhase2) > abs(maxPhase2)) {
        maxPhase2 = currentPhase2;
    }
}
        }
        vTaskDelay(pdMS_TO_TICKS(1)); // Minimaler Delay für Core-Stabilität
    }
}

// --- TASK 3: SERIELLE EINGABE (CORE 0, PRIO 1) ---
void TaskSerialInputLoop(void *pvParameters)
{
    for (;;)
    {
        if (Serial.available() > 0)
        {
            int inputVal = Serial.parseInt();
            if (inputVal != 0)
            {
                if (inputVal > 0 && inputVal > TorqMaxRange) inputVal = TorqMaxRange;
                else if (inputVal < 0 && inputVal < -TorqMaxRange) inputVal = -TorqMaxRange;

                TorqueVal = TorqueNeutral + inputVal;
                startTime = millis();
                motorRunning = true;
                Serial.println("#########################################################");
                Serial.print("Core 0: Motor Start Torq: ");
                Serial.println(TorqueVal);
            }
        }
        vTaskDelay(pdMS_TO_TICKS(100)); // Reicht für Tastatur locker aus
    }
}

void setup()
{
    Serial.begin(115200);
    delay(1000);
    Serial.println("Outlander Inverter  Tester @BAVARIANSuperGuy was here @MIT");
    Serial.println("Input: -35 bis 35 | Runtime: 4 Sek.");
     

    if (twai_driver_install(&g_config, &t_config, &f_config) == ESP_OK && twai_start() == ESP_OK)
    {
        Serial.println("CAN-Bus bereit.");
    }

    // Task 1: Senden auf CORE 1
    xTaskCreatePinnedToCore(TaskCanSendLoop, "CAN_Send", 4096, NULL, 5, &TaskCanSendHandle, 1);

    // Task 2: Empfangen auf CORE 0 (Hohe Priorität)
    xTaskCreatePinnedToCore(TaskCanRecvLoop, "CAN_Recv", 4096, NULL, 5, &TaskCanRecvHandle, 0);

    // Task 3: Serial Input auf CORE 0 (Niedrige Priorität)
    xTaskCreatePinnedToCore(TaskSerialInputLoop, "Serial_In", 4096, NULL, 1, &TaskSerialHandle, 0);
}

void loop()
{
    vTaskDelete(NULL);
}