mirror of https://github.com/rusefi/wideband.git
156 lines
3.9 KiB
C++
156 lines
3.9 KiB
C++
#include "can.h"
|
|
#include "hal.h"
|
|
|
|
#include "can_helper.h"
|
|
#include "heater_control.h"
|
|
#include "lambda_conversion.h"
|
|
#include "sampling.h"
|
|
#include "pump_dac.h"
|
|
#include "port.h"
|
|
|
|
Configuration configuration;
|
|
|
|
static THD_WORKING_AREA(waCanTxThread, 256);
|
|
void CanTxThread(void*)
|
|
{
|
|
while(1)
|
|
{
|
|
SendEmulatedAemXseries(configuration.CanIndexOffset);
|
|
|
|
chThdSleepMilliseconds(10);
|
|
}
|
|
}
|
|
|
|
static void SendAck()
|
|
{
|
|
CANTxFrame frame;
|
|
|
|
frame.IDE = CAN_IDE_EXT;
|
|
frame.EID = 0x727573; // ascii "rus"
|
|
frame.RTR = CAN_RTR_DATA;
|
|
frame.DLC = 0;
|
|
|
|
canTransmitTimeout(&CAND1, CAN_ANY_MAILBOX, &frame, TIME_INFINITE);
|
|
}
|
|
|
|
static THD_WORKING_AREA(waCanRxThread, 256);
|
|
void CanRxThread(void*)
|
|
{
|
|
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 (frame.IDE != CAN_IDE_EXT)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
if (frame.DLC == 2 && frame.EID == 0xEF5'0000) {
|
|
// This is status from ECU - battery voltage and heater enable signal
|
|
|
|
// data0 contains battery voltage in tenths of a volt
|
|
float vbatt = frame.data8[0] * 0.1f;
|
|
SetBatteryVoltage(vbatt);
|
|
|
|
// data1 contains heater enable bit
|
|
bool heaterAllowed = (frame.data8[1] & 0x1) == 0x1;
|
|
SetHeaterAllowed(heaterAllowed);
|
|
}
|
|
// If it's a bootloader entry request, reboot to the bootloader!
|
|
else if (frame.DLC == 0 && frame.EID == 0xEF0'0000)
|
|
{
|
|
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 && frame.EID == 0xEF4'0000)
|
|
{
|
|
auto newCfg = GetConfiguration();
|
|
newCfg.CanIndexOffset = frame.data8[0];
|
|
SetConfiguration(newCfg);
|
|
configuration = GetConfiguration();
|
|
SendAck();
|
|
}
|
|
}
|
|
}
|
|
|
|
void InitCan()
|
|
{
|
|
configuration = GetConfiguration();
|
|
|
|
canStart(&CAND1, &canConfig500);
|
|
chThdCreateStatic(waCanTxThread, sizeof(waCanTxThread), NORMALPRIO, CanTxThread, nullptr);
|
|
chThdCreateStatic(waCanRxThread, sizeof(waCanRxThread), NORMALPRIO - 4, CanRxThread, nullptr);
|
|
}
|
|
|
|
struct StandardDataFrame
|
|
{
|
|
uint16_t lambda;
|
|
uint16_t measuredResistance;
|
|
uint8_t pad[4];
|
|
};
|
|
|
|
#define SWAP_UINT16(x) (((x) << 8) | ((x) >> 8))
|
|
|
|
void SendEmulatedAemXseries(uint8_t idx) {
|
|
CanTxMessage frame(0x180 + idx, 8, true);
|
|
|
|
bool isValid = IsRunningClosedLoop();
|
|
|
|
float lambda = GetLambda();
|
|
uint16_t intLambda = lambda * 10000;
|
|
|
|
// swap endian
|
|
intLambda = SWAP_UINT16(intLambda);
|
|
*reinterpret_cast<uint16_t*>(&frame[0]) = intLambda;
|
|
|
|
// bit 1 = LSU 4.9 detected
|
|
// bit 7 = reading valid
|
|
frame[6] = 0x02 | (isValid ? 0x80 : 0x00);
|
|
|
|
// Hijack a reserved bit to indicate that we're NOT an AEM controller
|
|
frame[7] = 0x80;
|
|
|
|
// Now we embed some extra data for debug
|
|
// bytes 2-3 are officially oxygen percent
|
|
// byte 4 is officially supply voltage
|
|
|
|
// Report pump output PWM in byte 2, 0-255 for min to max target (128 = 0 current)
|
|
frame[2] = GetPumpOutputDuty() / 4;
|
|
|
|
// Report sensor ESR in byte 3, 4 ohm steps
|
|
int esrVal = (int)GetSensorInternalResistance() / 4;
|
|
|
|
// Clamp to uint8_t limits
|
|
if (esrVal > 255) {
|
|
esrVal = 255;
|
|
} else if (esrVal < 0) {
|
|
esrVal = 0;
|
|
}
|
|
|
|
frame[3] = esrVal;
|
|
|
|
// Report current nernst voltage in byte 4, 5mv steps
|
|
frame[4] = (int)(GetNernstDc() * 200);
|
|
}
|
|
|
|
void SendCanData(float lambda, uint16_t measuredResistance)
|
|
{
|
|
CanTxTyped<StandardDataFrame> frame(0x130);
|
|
|
|
frame.get().lambda = lambda * 10000;
|
|
frame.get().measuredResistance = measuredResistance;
|
|
}
|