diff --git a/firmware/hw_layer/drivers/sent/sent.cpp b/firmware/hw_layer/drivers/sent/sent.cpp index eab4ac9b90..04b8d8694c 100644 --- a/firmware/hw_layer/drivers/sent/sent.cpp +++ b/firmware/hw_layer/drivers/sent/sent.cpp @@ -50,6 +50,19 @@ /* Decoder */ /*==========================================================================*/ +/* Helpers for Msg manipulations */ +/* nibbles order: status, sig0_MSN, sig0_MidN, sig0_LSN, sig1_MSN, sig1_MidN, sig1_LSN, CRC */ +/* we shift rxReg left for 4 bits on each neable received and put newest nibble + * in [3:0] bits of rxReg, so when full message is received: + * CRC is [3:0] - nibble 7 + * status is [31:28] - nibble 0 + * sig0 is [27:16], sig1 is [15:4] */ +#define MsgGetNibble(msg, n) (((msg) >> (4 * (7 - (n)))) & 0xf) +#define MsgGetStat(msg) MsgGetNibble(msg, 0) +#define MsgGetSig0(msg) (((msg) >> (4 * 4)) & 0xfff) +#define MsgGetSig1(msg) (((msg) >> (1 * 4)) & 0xfff) +#define MsgGetCrc(msg) MsgGetNibble(msg, 7) + typedef enum { SENT_STATE_CALIB = 0, @@ -84,9 +97,10 @@ private: /* pulses skipped in init state while waiting for SYNC */ uint32_t initStatePulseCounter; - /* fast channel nibbles. - * TODO: rewrite with uint32_t shift register */ - uint8_t nibbles[SENT_MSG_PAYLOAD_SIZE]; + /* fast channel shift register*/ + uint32_t rxReg; + /* fast channel last received valid message */ + uint32_t rxLast; /* slow channel shift registers and flags */ uint16_t scMsgFlags; @@ -97,18 +111,13 @@ private: int SlowChannelDecoder(void); /* CRC */ - uint8_t crc4(uint8_t* pdata, uint16_t ndata); - uint8_t crc4_gm(uint8_t* pdata, uint16_t ndata); - uint8_t crc4_gm_v2(uint8_t* pdata, uint16_t ndata); + uint8_t crc4(uint32_t data); + uint8_t crc4_gm(uint32_t data); + uint8_t crc4_gm_v2(uint32_t data); void restart(void); public: - /* fast channel data */ - int32_t sig0; - int32_t sig1; - int8_t stat; - /* slow channel data */ struct { uint16_t data; @@ -123,11 +132,13 @@ public: /* Decoder */ int Decoder(uint16_t clocks); - /* get latest valid signal0 and signal1 + /* Get last raw message */ + int GetMsg(uint32_t* rx); + /* Unpack last valid message to status, signal0 and signal1 * Note: * sig0 is nibbles 0 .. 2, where nibble 0 is MSB * sig1 is niblles 5 .. 3, where niblle 5 is MSB */ - int GetSignals(uint16_t *pSig0, uint16_t *pSig1); + int GetSignals(uint8_t *pStat, uint16_t *pSig0, uint16_t *pSig1); /* Show status */ void Info(void); @@ -228,6 +239,7 @@ int sent_channel::Decoder(uint16_t clocks) /* measured tick interval will be used until next sync pulse */ tickPerUnit = (clocks + (SENT_SYNC_INTERVAL + SENT_OFFSET_INTERVAL) / 2) / (SENT_SYNC_INTERVAL + SENT_OFFSET_INTERVAL); + rxReg = 0; state = SENT_STATE_STATUS; } else @@ -259,7 +271,7 @@ int sent_channel::Decoder(uint16_t clocks) case SENT_STATE_CRC: if(interval <= SENT_MAX_INTERVAL) { - nibbles[state - SENT_STATE_STATUS] = interval; + rxReg = (rxReg << 4) | (uint32_t)interval; if (state != SENT_STATE_CRC) { @@ -273,24 +285,13 @@ int sent_channel::Decoder(uint16_t clocks) #endif // SENT_STATISTIC_COUNTERS /* CRC check */ /* TODO: find correct way to calculate CRC */ - if ((nibbles[7] == crc4(nibbles, 7)) || - (nibbles[7] == crc4_gm(nibbles + 1, 6)) || - (nibbles[7] == crc4_gm_v2(nibbles + 1, 6))) + if ((MsgGetCrc(rxReg) == crc4(rxReg)) || + (MsgGetCrc(rxReg) == crc4_gm(rxReg)) || + (MsgGetCrc(rxReg) == crc4_gm_v2(rxReg))) { - /* Full packet with correct CRC has been received - * NOTE different MSB packing for sig0 and sig1 - * is it protocol-defined or device-specific? - * looks like some devices send 16 + 8 bit, not 12 + 12 */ - sig0 = - (nibbles[1 + 0] << 8) | - (nibbles[1 + 1] << 4) | - (nibbles[1 + 2] << 0); - sig1 = - (nibbles[1 + 3] << 0) | - (nibbles[1 + 4] << 4) | - (nibbles[1 + 5] << 8); - stat = - nibbles[0]; + /* Full packet with correct CRC has been received */ + rxLast = rxReg; + /* TODO: add timestamp? */ ret = 1; } else @@ -328,14 +329,45 @@ int sent_channel::Decoder(uint16_t clocks) return ret; } -int sent_channel::GetSignals(uint16_t *pSig0, uint16_t *pSig1) +int sent_channel::GetMsg(uint32_t* rx) { + if (rx) { + *rx = rxLast; + } + + /* TODO: add check if any packet was received */ + /* TODO: add check for time since last message reseived */ + return 0; +} + +int sent_channel::GetSignals(uint8_t *pStat, uint16_t *pSig0, uint16_t *pSig1) +{ + uint32_t rx; + int ret = GetMsg(&rx); + + if (ret < 0) { + return ret; + } + + /* NOTE different MSB packing for sig0 and sig1 + * is it protocol-defined or device-specific? + * Also looks like some devices send 16 + 8 bit, not 12 + 12 */ + if (pStat) { + *pStat = MsgGetStat(rx); + } + if (pSig0) { - *pSig0 = sig0; + uint16_t tmp = MsgGetSig0(rx); + *pSig0 = tmp; } if (pSig1) { - *pSig1 = sig1; + uint16_t tmp = MsgGetSig1(rx); + /* swap */ + tmp = ((tmp >> 8) & 0x00f) | + ((tmp << 8) & 0xf00) | + (tmp & 0x0f0); + *pSig1 = tmp; } return 0; @@ -371,8 +403,8 @@ int sent_channel::SlowChannelStore(uint8_t id, uint16_t data) int sent_channel::SlowChannelDecoder() { /* bit 2 and bit 3 from status nibble are used to transfer short messages */ - bool b2 = !!(nibbles[0] & (1 << 2)); - bool b3 = !!(nibbles[0] & (1 << 3)); + bool b2 = !!(MsgGetStat(rxLast) & BIT(2)); + bool b3 = !!(MsgGetStat(rxLast) & BIT(3)); /* shift in new data */ scShift2 = (scShift2 << 1) | b2; @@ -430,56 +462,51 @@ int sent_channel::SlowChannelDecoder() return 0; } -/* This CRC works for Si7215 for WHOLE message expect last nibble (CRC) */ -uint8_t sent_channel::crc4(uint8_t* pdata, uint16_t ndata) +/* This is correct for Si7215 */ +/* This CRC is calculated for WHOLE message expect last nibble (CRC) */ +uint8_t sent_channel::crc4(uint32_t data) { size_t i; uint8_t crc = SENT_CRC_SEED; // initialize checksum with seed "0101" const uint8_t CrcLookup[16] = {0, 13, 7, 10, 14, 3, 9, 4, 1, 12, 6, 11, 15, 2, 8, 5}; - for (i = 0; i < ndata; i++) - { - crc = crc ^ pdata[i]; + for (i = 0; i < 7; i++) { + crc = crc ^ MsgGetNibble(data, i); crc = CrcLookup[crc]; } return crc; } -/* This CRC works for GM pressure sensor for message minus status nibble and minus CRC nibble */ -/* TODO: double check and use same CRC routine? */ +/* TODO: double check two following and use same CRC routine? */ /* This is correct for GM throttle body */ -uint8_t sent_channel::crc4_gm(uint8_t* pdata, uint16_t ndata) +/* This CRC is calculated for message expect status nibble and minus CRC nibble */ +uint8_t sent_channel::crc4_gm(uint32_t data) { size_t i; uint8_t crc = SENT_CRC_SEED; // initialize checksum with seed "0101" const uint8_t CrcLookup[16] = {0, 13, 7, 10, 14, 3, 9, 4, 1, 12, 6, 11, 15, 2, 8, 5}; - for (i = 0; i < ndata; i++) - { + for (i = 1; i < 7; i++) { crc = CrcLookup[crc]; - crc = (crc ^ pdata[i]) & 0x0F; - //crc = pdata[i] ^ CrcLookup[crc]; + crc = (crc ^ MsgGetNibble(data, i)) & 0xf; } - // One more round with 0 as input - //crc = CrcLookup[crc]; return crc; } /* This is correct for GDI fuel pressure sensor */ -uint8_t sent_channel::crc4_gm_v2(uint8_t* pdata, uint16_t ndata) +/* This CRC is calculated for message expect status nibble and minus CRC nibble */ +uint8_t sent_channel::crc4_gm_v2(uint32_t data) { size_t i; uint8_t crc = SENT_CRC_SEED; // initialize checksum with seed "0101" const uint8_t CrcLookup[16] = {0, 13, 7, 10, 14, 3, 9, 4, 1, 12, 6, 11, 15, 2, 8, 5}; - for (i = 0; i < ndata; i++) - { + for (i = 1; i < 7; i++) { crc = CrcLookup[crc]; - crc = (crc ^ pdata[i]) & 0x0F; - //crc = pdata[i] ^ CrcLookup[crc]; + crc = (crc ^ MsgGetNibble(data, i)) & 0xf; } // One more round with 0 as input crc = CrcLookup[crc]; @@ -491,9 +518,15 @@ void sent_channel::Info(void) { int i; + uint8_t stat; + uint16_t sig0, sig1; + efiPrintf("Unit time %d timer ticks", tickPerUnit); efiPrintf("Total pulses %d", pulseCounter); - efiPrintf("Last fast msg Status 0x%01x Sig0 0x%03x Sig1 0x%03x", stat, sig0, sig1); + + if (GetSignals(&stat, &sig0, &sig1) == 0) { + efiPrintf("Last valid fast msg Status 0x%01x Sig0 0x%03x Sig1 0x%03x", stat, sig0, sig1); + } if (scMsgFlags) { efiPrintf("Slow channels:");