wideband/firmware/can.cpp

#include "hal.h"

#include "can.h"

#include "status.h"
#include "can_helper.h"
#include "heater_control.h"
#include "lambda_conversion.h"
#include "sampling.h"
#include "pump_dac.h"
#include "port.h"
#include "pump_control.h"
#include "wideband_config.h"

#include <rusefi/math.h>

// this same header is imported by rusEFI to get struct layouts and firmware version
#include "../for_rusefi/wideband_can.h"

static Configuration* configuration;

static THD_WORKING_AREA(waCanTxThread, 512);
void CanTxThread(void*)
{
    chRegSetThreadName("CAN Tx");

    // Current system time.
    systime_t prev = chVTGetSystemTime();

    while (1)
    {
        for (int ch = 0; ch < AFR_CHANNELS; ch++)
        {
            SendCanForChannel(ch);
        }

        prev = chThdSleepUntilWindowed(prev, chTimeAddX(prev, TIME_MS2I(WBO_TX_PERIOD_MS)));
    }
}

static void SendAck()
{
    CANTxFrame frame;

#ifdef STM32G4XX
    frame.common.RTR = 0;
#else // Not CAN FD
    frame.RTR = CAN_RTR_DATA;
#endif

    CAN_EXT(frame) = 1;
    CAN_EID(frame) = WB_ACK;
    frame.DLC = 0;

    canTransmitTimeout(&CAND1, CAN_ANY_MAILBOX, &frame, TIME_INFINITE);
}

// Start in Unknown state. If no CAN message is ever received, we operate
// on internal battery sense etc.
static HeaterAllow heaterAllow = HeaterAllow::Unknown;
static float remoteBatteryVoltage = 0;

static THD_WORKING_AREA(waCanRxThread, 512);
void CanRxThread(void*)
{
    chRegSetThreadName("CAN Rx");

    while (1)
    {
        CANRxFrame frame;
        msg_t msg = canReceiveTimeout(&CAND1, CAN_ANY_MAILBOX, &frame, TIME_INFINITE);

        // Ignore non-ok results...
        if (msg != MSG_OK)
        {
            continue;
        }

        // Ignore std frames, only listen to ext
        if (!CAN_EXT(frame))
        {
            continue;
        }

        // Ignore not ours frames
        if (WB_MSG_GET_HEADER(CAN_ID(frame)) != WB_BL_HEADER)
        {
            continue;
        }

        if (frame.DLC >= 2 && CAN_ID(frame) == WB_MSG_ECU_STATUS)
        {
            // This is status from ECU
            // - battery voltage
            // - heater enable signal
            // - optionally pump control gain

            // data0 contains battery voltage in tenths of a volt
            float vbatt = frame.data8[0] * 0.1f;
            remoteBatteryVoltage = vbatt;

            if (vbatt < HEATER_BATTERY_OFF_VOLTAGE)
            {
                // Supply voltage too low to heat, override allow state
                heaterAllow = HeaterAllow::NotAllowed;
            }
            else
            {
                // Voltage is high enough, check if ECU says we're allowed to heat
                // data1 contains heater enable bit
                if ((frame.data8[1] & 0x1) == 0x1)
                {
                    heaterAllow = HeaterAllow::Allowed;
                }
                else
                {
                    heaterAllow = HeaterAllow::NotAllowed;
                }
            }

            if (frame.DLC >= 3)
            {
                // data2 contains pump controller gain in percent (0-200)
                float pumpGain = frame.data8[2] * 0.01f;
                SetPumpGainAdjust(clampF(0, pumpGain, 1));
            }
        }
        // If it's a bootloader entry request, reboot to the bootloader!
        // DLC=1 required: data[0] is 0xFF (all) or a specific controller index.
        // DLC=0 is ignored so that only the targeted controller enters bootloader
        // when multiple controllers share a bus.
        else if (frame.DLC == 1 && CAN_ID(frame) == WB_BL_ENTER)
        {
            if (frame.data8[0] == 0xFF || frame.data8[0] == GetConfiguration()->CanIndexOffset)
            {
                SendAck();

                // Let the message get out before we reset the chip
                chThdSleep(50);

                NVIC_SystemReset();
            }
        }
        // Check if it's an "index set" message
        else if (frame.DLC == 1 && CAN_ID(frame) == WB_MSG_SET_INDEX)
        {
            configuration = GetConfiguration();
            configuration->CanIndexOffset = frame.data8[0];
            SetConfiguration();
            SendAck();
        }
    }
}

HeaterAllow GetHeaterAllowed()
{
    return heaterAllow;
}

float GetRemoteBatteryVoltage()
{
    return remoteBatteryVoltage;
}

void InitCan()
{
    configuration = GetConfiguration();

    canStart(&CAND1, &GetCanConfig());
    chThdCreateStatic(waCanTxThread, sizeof(waCanTxThread), NORMALPRIO, CanTxThread, nullptr);
    chThdCreateStatic(waCanRxThread, sizeof(waCanRxThread), NORMALPRIO - 4, CanRxThread, nullptr);
}

void SendRusefiFormat(uint8_t ch)
{
    auto baseAddress = WB_DATA_BASE_ADDR + 2 * (ch + configuration->CanIndexOffset);

    const auto& sampler = GetSampler(ch);
    const auto& heater = GetHeaterController(ch);

    auto nernstDc = sampler.GetNernstDc();
    auto pumpDuty = GetPumpOutputDuty(ch);
    auto lambda = GetLambda(ch);

    // Lambda is valid if:
    // 1. Nernst voltage is near target
    // 2. Lambda is >0.6 (sensor isn't specified below that)
    bool lambdaValid = nernstDc > (NERNST_TARGET - 0.1f) && nernstDc < (NERNST_TARGET + 0.1f) && lambda > 0.6f;

    {
        CanTxTyped<wbo::StandardData> frame(baseAddress + 0);

        // The same header is imported by the ECU and checked against this data in the frame
        frame.get().Version = RUSEFI_WIDEBAND_VERSION;

        uint16_t lambdaInt = lambdaValid ? (lambda * 10000) : 0;
        frame.get().Lambda = lambdaInt;
        frame.get().TemperatureC = sampler.GetSensorTemperature();
        bool heaterClosedLoop = heater.IsRunningClosedLoop();
        frame.get().Valid = (heaterClosedLoop && lambdaValid) ? 0x01 : 0x00;
    }

    {
        CanTxTyped<wbo::DiagData> frame(baseAddress + 1);

        frame.get().Esr = sampler.GetSensorInternalResistance();
        frame.get().NernstDc = nernstDc * 1000;
        frame.get().PumpDuty = pumpDuty * 255;
        frame.get().status = GetCurrentStatus(ch);
        frame.get().HeaterDuty = GetHeaterDuty(ch) * 255;
    }
}

// Weak link so boards can override it
__attribute__((weak)) void SendCanForChannel(uint8_t ch)
{
    SendRusefiFormat(ch);
}