295 lines
9.6 KiB
C++
295 lines
9.6 KiB
C++
#include "can.h"
|
|
#include "hal.h"
|
|
|
|
#include "efifeatures.h"
|
|
|
|
#include <cstdint>
|
|
#include <cstring>
|
|
#include "sent_canbus_protocol.h"
|
|
|
|
#include "fault.h"
|
|
#include "io_pins.h"
|
|
#include "persistence.h"
|
|
#include "can_common.h"
|
|
#include "pt2001impl.h"
|
|
#include "chprintf.h"
|
|
#include "sent.h"
|
|
#include "misc.h"
|
|
|
|
#define GDI4_CAN_SET_TAG 0x78
|
|
#include <rusefi/manifest.h>
|
|
|
|
// Decimal hex date presented as hex
|
|
static char VERSION[] = {compilationYear() / 100, compilationYear() % 100, compilationMonth(), compilationDay()};
|
|
|
|
extern GDIConfiguration configuration;
|
|
extern Pt2001 chips[EFI_PT2001_CHIPS];
|
|
extern bool isOverallHappyStatus;
|
|
|
|
static const CANConfig canConfig500 =
|
|
{
|
|
CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP,
|
|
CAN_BTR_SJW(0) | CAN_BTR_BRP(2) | CAN_BTR_TS1(12) | CAN_BTR_TS2(1),
|
|
};
|
|
|
|
#define CAN_TX_TIMEOUT_100_MS TIME_MS2I(100)
|
|
|
|
int canWriteOk = 0;
|
|
int canWriteNotOk = 0;
|
|
//static char printBuffer[200];
|
|
|
|
static void countTxResult(msg_t msg) {
|
|
if (msg == MSG_OK) {
|
|
canWriteOk++;
|
|
} else {
|
|
canWriteNotOk++;
|
|
}
|
|
}
|
|
|
|
int canGetOutputCanIdBase(size_t chip)
|
|
{
|
|
return (configuration.outputCanID + (boardGetId() + chip) * GDI4_BASE_ADDRESS_OFFSET);
|
|
}
|
|
|
|
int canGetInputCanIdBase(size_t chip)
|
|
{
|
|
return (configuration.inputCanID + (boardGetId() + chip) * GDI4_BASE_ADDRESS_OFFSET);
|
|
}
|
|
|
|
void SendSomething(size_t chip, int baseID) {
|
|
CANTxFrame m_frame;
|
|
|
|
m_frame.IDE = CAN_IDE_EXT;
|
|
m_frame.SID = 0;
|
|
m_frame.EID = baseID + 0;
|
|
m_frame.RTR = CAN_RTR_DATA;
|
|
m_frame.DLC = 8;
|
|
memset(m_frame.data8, 0, sizeof(m_frame.data8));
|
|
|
|
m_frame.data8[0] = configuration.inputCanID;
|
|
m_frame.data8[1] = configuration.updateCounter;
|
|
m_frame.data8[2] = isOverallHappyStatus;
|
|
m_frame.data8[6] = (int)chips[chip].fault;
|
|
m_frame.data8[7] = GDI4_MAGIC;
|
|
|
|
msg_t msg = canTransmitTimeout(&CAND1, CAN_ANY_MAILBOX, &m_frame, CAN_TX_TIMEOUT_100_MS);
|
|
countTxResult(msg);
|
|
}
|
|
|
|
static void sendOutConfiguration(int baseID) {
|
|
CANTxFrame m_frame;
|
|
|
|
m_frame.IDE = CAN_IDE_EXT;
|
|
m_frame.SID = 0;
|
|
m_frame.RTR = CAN_RTR_DATA;
|
|
memset(m_frame.data8, 0, sizeof(m_frame.data8));
|
|
|
|
// CanConfiguration1
|
|
m_frame.DLC = 8;
|
|
m_frame.data16[0] = configuration.BoostVoltage;
|
|
m_frame.data16[1] = float2short128(configuration.BoostCurrent);
|
|
m_frame.data16[2] = configuration.TBoostMin;
|
|
m_frame.data16[3] = configuration.TBoostMax;
|
|
m_frame.EID = baseID + 1;
|
|
msg_t msg = canTransmitTimeout(&CAND1, CAN_ANY_MAILBOX, &m_frame, CAN_TX_TIMEOUT_100_MS);
|
|
countTxResult(msg);
|
|
|
|
// CanConfiguration2
|
|
m_frame.DLC = 8;
|
|
m_frame.data16[0] = float2short128(configuration.PeakCurrent);
|
|
m_frame.data16[1] = configuration.TpeakDuration;
|
|
m_frame.data16[2] = configuration.TpeakOff;
|
|
m_frame.data16[3] = configuration.Tbypass;
|
|
m_frame.EID = baseID + 2;
|
|
msg = canTransmitTimeout(&CAND1, CAN_ANY_MAILBOX, &m_frame, CAN_TX_TIMEOUT_100_MS);
|
|
countTxResult(msg);
|
|
|
|
// CanConfiguration3
|
|
m_frame.DLC = 8;
|
|
m_frame.data16[0] = float2short128(configuration.HoldCurrent);
|
|
m_frame.data16[1] = configuration.TholdOff;
|
|
m_frame.data16[2] = configuration.THoldDuration;
|
|
m_frame.data16[3] = float2short128(configuration.PumpPeakCurrent);
|
|
m_frame.EID = baseID + 3;
|
|
msg = canTransmitTimeout(&CAND1, CAN_ANY_MAILBOX, &m_frame, CAN_TX_TIMEOUT_100_MS);
|
|
countTxResult(msg);
|
|
|
|
// CanConfiguration4
|
|
m_frame.DLC = 2;
|
|
m_frame.data16[0] = float2short128(configuration.PumpHoldCurrent);
|
|
m_frame.EID = baseID + 4;
|
|
msg = canTransmitTimeout(&CAND1, CAN_ANY_MAILBOX, &m_frame, CAN_TX_TIMEOUT_100_MS);
|
|
countTxResult(msg);
|
|
}
|
|
|
|
static void sendOutVersion(int baseID) {
|
|
CANTxFrame m_frame;
|
|
|
|
m_frame.IDE = CAN_IDE_EXT;
|
|
m_frame.SID = 0;
|
|
m_frame.EID = baseID + 5;
|
|
m_frame.RTR = CAN_RTR_DATA;
|
|
m_frame.DLC = sizeof(VERSION);
|
|
memcpy(m_frame.data8, VERSION, sizeof(VERSION));
|
|
msg_t msg = canTransmitTimeout(&CAND1, CAN_ANY_MAILBOX, &m_frame, CAN_TX_TIMEOUT_100_MS);
|
|
countTxResult(msg);
|
|
}
|
|
|
|
static void sendOutSentData(int baseID) {
|
|
// TODO: start using CanTxTyped helper
|
|
CANTxFrame m_frame;
|
|
|
|
m_frame.IDE = CAN_IDE_EXT;
|
|
m_frame.SID = 0;
|
|
m_frame.EID = baseID + 6;
|
|
m_frame.RTR = CAN_RTR_DATA;
|
|
m_frame.DLC = 8;
|
|
|
|
// See scaled_high_pressure scaled_channel, 0.1 bar resolution
|
|
uint16_t press = GmPressureGetPressure() * GM_SENT_CANBUS_SCALE_PRESSURE;
|
|
// See scaled_temperature scaled_channel, 0.01 deg C resolution
|
|
int16_t temp = GmPressureGetTemperature() * GM_SENT_CANBUS_SCALE_TEMPERATURE;
|
|
|
|
m_frame.data16[0] = press;
|
|
m_frame.data16[1] = temp;
|
|
m_frame.data32[1] = 0xdeadbeef;
|
|
|
|
msg_t msg = canTransmitTimeout(&CAND1, CAN_ANY_MAILBOX, &m_frame, CAN_TX_TIMEOUT_100_MS);
|
|
countTxResult(msg);
|
|
}
|
|
|
|
#define CAN_TX_PERIOD_MS 100
|
|
|
|
static int intTxCounter = 0;
|
|
|
|
static THD_WORKING_AREA(waCanTxThread, 256);
|
|
void CanTxThread(void*)
|
|
{
|
|
chRegSetThreadName("CAN TX");
|
|
|
|
while (1) {
|
|
intTxCounter++;
|
|
chThdSleepMilliseconds(1000 / CAN_TX_PERIOD_MS);
|
|
|
|
if (configuration.outputCanID == 0) {
|
|
continue; // we were told to be silent
|
|
}
|
|
|
|
for (size_t i = 0; i < EFI_PT2001_CHIPS; i++) {
|
|
// keep constant while sending whole banch of messages
|
|
int outID = canGetOutputCanIdBase(i);
|
|
|
|
if (intTxCounter % (1000 / CAN_TX_PERIOD_MS) == 0) {
|
|
sendOutConfiguration(outID);
|
|
}
|
|
if (intTxCounter % (1000 / CAN_TX_PERIOD_MS) == 0) {
|
|
sendOutVersion(outID);
|
|
}
|
|
|
|
SendSomething(i, outID);
|
|
|
|
if (i == 0) {
|
|
sendOutSentData(outID);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
static int getInt(CANRxFrame *frame, int offset) {
|
|
return frame->data8[offset + 1] * 256 + frame->data8[offset];
|
|
}
|
|
|
|
static float getFloat(CANRxFrame *frame, int offset) {
|
|
int value = getInt(frame, offset);
|
|
return short2float128(value);
|
|
}
|
|
|
|
#define ASSIGN_IF_CHANGED(oldValue, newValue) \
|
|
if ((oldValue) != (newValue)) { \
|
|
oldValue = (newValue); \
|
|
withNewValue = true; \
|
|
}
|
|
|
|
static THD_WORKING_AREA(waCanRxThread, 256);
|
|
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 (frame.IDE != CAN_IDE_EXT) {
|
|
continue;
|
|
}
|
|
|
|
// ignore packets not starting with magic byte or of unexpected length
|
|
if (frame.data8[0] != GDI4_CAN_SET_TAG || frame.DLC != 7) {
|
|
continue;
|
|
}
|
|
|
|
for (size_t i = 0; i < EFI_PT2001_CHIPS; i++) {
|
|
// size_t writeCount = 0;
|
|
|
|
// writeCount = chsnprintf(printBuffer, sizeof(printBuffer), "eid=%d data[0]=%d dlc=%d\n\n\n\n\n\n\n",
|
|
// frame.EID,
|
|
// frame.data8[0],
|
|
// frame.DLC);
|
|
|
|
bool withNewValue = false;
|
|
int inputID = canGetInputCanIdBase(i);
|
|
// TODO: valudate DLC and IDE
|
|
if (frame.EID == inputID) {
|
|
ASSIGN_IF_CHANGED(configuration.BoostVoltage, getInt(&frame, 1));
|
|
ASSIGN_IF_CHANGED(configuration.BoostCurrent, getFloat(&frame, 3));
|
|
ASSIGN_IF_CHANGED(configuration.TBoostMin, getInt(&frame, 5));
|
|
} else if (frame.EID == inputID + 1) {
|
|
ASSIGN_IF_CHANGED(configuration.TBoostMax, getInt(&frame, 1));
|
|
ASSIGN_IF_CHANGED(configuration.PeakCurrent, getFloat(&frame, 3));
|
|
ASSIGN_IF_CHANGED(configuration.TpeakDuration, getInt(&frame, 5));
|
|
} else if (frame.EID == inputID + 2) {
|
|
ASSIGN_IF_CHANGED(configuration.TpeakOff, getInt(&frame, 1));
|
|
ASSIGN_IF_CHANGED(configuration.Tbypass, getInt(&frame, 3));
|
|
ASSIGN_IF_CHANGED(configuration.HoldCurrent, getFloat(&frame, 5));
|
|
} else if (frame.EID == inputID + 3) {
|
|
ASSIGN_IF_CHANGED(configuration.TholdOff, getInt(&frame, 1));
|
|
ASSIGN_IF_CHANGED(configuration.THoldDuration, getInt(&frame, 3));
|
|
ASSIGN_IF_CHANGED(configuration.PumpPeakCurrent, getFloat(&frame, 5));
|
|
} else if (frame.EID == inputID + 4) {
|
|
ASSIGN_IF_CHANGED(configuration.PumpHoldCurrent, getFloat(&frame, 1));
|
|
ASSIGN_IF_CHANGED(configuration.outputCanID, getInt(&frame, 3));
|
|
}
|
|
if (withNewValue) {
|
|
saveConfiguration();
|
|
chips[i].restart();
|
|
}
|
|
// if (writeCount > 0)
|
|
// uartStartSend(&UARTD1, writeCount, printBuffer);
|
|
}
|
|
|
|
chThdSleepMilliseconds(100);
|
|
}
|
|
}
|
|
|
|
void InitCan()
|
|
{
|
|
canStart(&CAND1, &canConfig500);
|
|
|
|
// CAN TX
|
|
palSetPadMode(CAN_GPIO_PORT,CAN_TX_PIN, PAL_MODE_STM32_ALTERNATE_PUSHPULL );
|
|
// CAN RX
|
|
palSetPadMode(CAN_GPIO_PORT,CAN_RX_PIN, PAL_MODE_INPUT_PULLUP );
|
|
|
|
chThdCreateStatic(waCanTxThread, sizeof(waCanTxThread), NORMALPRIO, CanTxThread, nullptr);
|
|
chThdCreateStatic(waCanRxThread, sizeof(waCanRxThread), NORMALPRIO - 4, CanRxThread, nullptr);
|
|
}
|
|
|
|
//#define SWAP_UINT16(x) (((x) << 8) | ((x) >> 8))
|
|
|