diff --git a/SENT-box/firmware/sent.cpp b/SENT-box/firmware/sent.cpp index 5dbf108..de54953 100644 --- a/SENT-box/firmware/sent.cpp +++ b/SENT-box/firmware/sent.cpp @@ -11,29 +11,23 @@ #include "sent.h" // Sent SM status arr -SM_SENT_enum sentSMstate[4] = {SM_SENT_INIT_STATE}; +SM_SENT_enum sentSMstate[SENT_CHANNELS_NUM] = {SM_SENT_INIT_STATE}; // Value sensor arr -uint16_t sentValArr[4] = {0}; +uint16_t sentValArr[SENT_CHANNELS_NUM] = {0}; // -uint16_t sentTempValArr[4] = {0}; +uint16_t sentTempValArr[SENT_CHANNELS_NUM] = {0}; // Received nibbles arr -uint8_t sentTempNibblArr[4][6] = {0}; +uint8_t sentTempNibblArr[SENT_CHANNELS_NUM][SENT_MSG_PAYLOAD_SIZE] = {0}; // Sensor status arr -uint8_t sentStat[4] = {0}; +uint8_t sentStat[SENT_CHANNELS_NUM] = {0}; // Roll counter arr -uint8_t sentRollCnt[4] = {0}; -uint8_t sentRollCntPrev[4] = {0}; - -// -//uint16_t val_prev[4]; - -// Received msg counter -uint32_t val_res_cnt = 0; +uint8_t sentRollCnt[SENT_CHANNELS_NUM] = {0}; +uint8_t sentRollCntPrev[SENT_CHANNELS_NUM] = {0}; // Error counters uint32_t sentIntervalErr = 0; @@ -42,30 +36,30 @@ uint32_t sentStatusErr = 0; uint32_t sentRollErrCnt = 0; uint32_t sentCrcErrCnt = 0; -#if 0 +static const uint8_t SENT_CRC4_tbl[16] = +{ + 0, 13, 7, 10, 14, 3, 9, 4, 1, 12, 6, 11, 15, 2, 8, 5 +}; + +#if SENT_ERR_PERCENT +// Received msg counter +uint32_t val_res_cnt = 0; + float err_per = 0; - -uint8_t val_min_cnt = 0; -uint8_t val_max_cnt = 0; - -uint16_t sentMaxSyncVal = 0; -uint16_t sentMinSyncVal = 50000; - -uint8_t senDataReady = 0; #endif static void icuperiodcb_in1(ICUDriver *icup); static void icuperiodcb_in2(ICUDriver *icup); static void icuperiodcb_in3(ICUDriver *icup); -static void SENT_ISR_Handler(uint8_t ch, uint16_t val); +static void SENT_ISR_Handler(uint8_t ch, uint16_t val_res); uint8_t sent_crc4(uint8_t* pdata, uint16_t ndata); // Sent input1 - TIM4 CH1 - PB6 static ICUConfig icucfg_in1 = { ICU_INPUT_ACTIVE_HIGH, - 400000, /* 400kHz ICU clock frequency - 2.5 us. */ + SENT_ICU_FREQ, /* 400kHz ICU clock frequency - 2.5 us. */ NULL, icuperiodcb_in1, NULL, @@ -78,7 +72,7 @@ static ICUConfig icucfg_in1 = static ICUConfig icucfg_in2 = { ICU_INPUT_ACTIVE_HIGH, - 400000, /* 400kHz ICU clock frequency - 2.5 us. */ + SENT_ICU_FREQ, /* 400kHz ICU clock frequency - 2.5 us. */ NULL, icuperiodcb_in2, NULL, @@ -91,7 +85,7 @@ static ICUConfig icucfg_in2 = static ICUConfig icucfg_in3 = { ICU_INPUT_ACTIVE_HIGH, - 400000, /* 400kHz ICU clock frequency - 2.5 us. */ + SENT_ICU_FREQ, /* 400kHz ICU clock frequency - 2.5 us. */ NULL, icuperiodcb_in3, NULL, @@ -100,265 +94,189 @@ static ICUConfig icucfg_in3 = 0xFFFFFFFFU }; -icucnt_t last_period; - static void icuperiodcb_in1(ICUDriver *icup) { - - //last_period = icuGetPeriodX(icup); - - SENT_ISR_Handler(0, icuGetPeriodX(icup)); + SENT_ISR_Handler(SENT_CH1, icuGetPeriodX(icup)); } static void icuperiodcb_in2(ICUDriver *icup) { - - //last_period = icuGetPeriodX(icup); - - SENT_ISR_Handler(1, icuGetPeriodX(icup)); + SENT_ISR_Handler(SENT_CH2, icuGetPeriodX(icup)); } static void icuperiodcb_in3(ICUDriver *icup) { - - //last_period = icuGetPeriodX(icup); - - SENT_ISR_Handler(2, icuGetPeriodX(icup)); + SENT_ISR_Handler(SENT_CH3, icuGetPeriodX(icup)); } void InitSent() { - icuStart(&ICUD4, &icucfg_in1); - icuStartCapture(&ICUD4); - icuEnableNotifications(&ICUD4); + icuStart(&SENT_ICUD_CH1, &icucfg_in1); + icuStartCapture(&SENT_ICUD_CH1); + icuEnableNotifications(&SENT_ICUD_CH1); + + icuStart(&SENT_ICUD_CH2, &icucfg_in2); + icuStartCapture(&SENT_ICUD_CH2); + icuEnableNotifications(&SENT_ICUD_CH2); + + icuStart(&SENT_ICUD_CH3, &icucfg_in3); + icuStartCapture(&SENT_ICUD_CH3); + icuEnableNotifications(&SENT_ICUD_CH3); - icuStart(&ICUD3, &icucfg_in2); - icuStartCapture(&ICUD3); - icuEnableNotifications(&ICUD3); -#if 0 - icuStart(&ICUD1, &icucfg_in3); - icuStartCapture(&ICUD1); - icuEnableNotifications(&ICUD1); -#endif } -uint16_t SentGetPeriodValue(void) +static void SENT_ISR_Handler(uint8_t ch, uint16_t val_res) { - return (last_period >> 1); -} + val_res >>= 1; -static void SENT_ISR_Handler(uint8_t ch, uint16_t val) -{ - uint16_t val_res; - uint8_t sentCrc; - -#if 0 - if(val > val_prev[ch]) - { - val_res = val - val_prev[ch]; - } - else - { - val_res = SENT_PERIOD - val_prev[ch] + val; - } - - val_prev[ch] = val; - - - val_res = val_res >> 1; -#else - val_res = val >> 1; -#endif - - if(val_res < 12) - { + if(val_res < SENT_MIN_INTERVAL) + {// sent interval less than min interval sentIntervalErr++; } else { - val_res = val_res - 12; + val_res -= SENT_OFFSET_INTERVAL; - -#if 0 - sentValArr[ch] = val_res; - - if(val_res > 50) - { - if(val_res > sentMaxSyncVal) - { - if(val_max_cnt < 5) - { - val_max_cnt++; - } - else - { - sentMaxSyncVal = val_res; - } - } - - if(val_res < sentMinSyncVal) - { - if(val_min_cnt < 5) - { - val_min_cnt++; - } - else - { - sentMinSyncVal = val_res; - } - } - - val_res_cnt++; - - if(val_res != 56) // 56 - { - sentSyncErr++; - - err_per = ((float)val_res_err_cnt/(float)val_res_cnt)*100; - } - } -#endif - switch(sentSMstate[ch]) + if((val_res != SENT_SYNC_INTERVAL) && (val_res > SENT_MAX_INTERVAL)) + {// sent interval more than max interval + sentIntervalErr++; + } + else { - case SM_SENT_INIT_STATE: - val_res_cnt++; + switch(sentSMstate[ch]) + { + case SM_SENT_INIT_STATE: + #if SENT_ERR_PERCENT + val_res_cnt++; + #endif + + if(val_res == SENT_SYNC_INTERVAL) + {// sync interval - 56 ticks + sentSMstate[ch] = SM_SENT_STATUS_STATE; + } + break; + + case SM_SENT_SYNC_STATE: + #if SENT_ERR_PERCENT + val_res_cnt++; + #endif + + if(val_res == SENT_SYNC_INTERVAL) + {// sync interval - 56 ticks + sentTempValArr[ch] = 0; - if(val_res == 44) - {// sync interval - 56 ticks sentSMstate[ch] = SM_SENT_STATUS_STATE; - } - break; + } + else + { + // Increment sync interval err count + sentSyncErr++; - case SM_SENT_SYNC_STATE: - val_res_cnt++; + #if SENT_ERR_PERCENT + // Calc err percentage + err_per = ((float)val_res_err_cnt/(float)val_res_cnt)*100; + #endif + } + break; - if(val_res == 44) - {// sync interval - 56 ticks - sentTempValArr[ch] = 0; + case SM_SENT_STATUS_STATE: + // status interval + sentStat[ch] = val_res; - sentSMstate[ch] = SM_SENT_STATUS_STATE; - } - else - { - // Increment sync interval err count - sentSyncErr++; + if(sentStat[ch]) + { + sentStatusErr++; + } - // Calc err percentage - // err_per = ((float)val_res_err_cnt/(float)val_res_cnt)*100; - } - break; + sentSMstate[ch] = SM_SENT_SIG1_DATA1_STATE; + break; - case SM_SENT_STATUS_STATE: - // status interval - sentStat[ch] = val_res; + case SM_SENT_SIG1_DATA1_STATE: + sentTempNibblArr[ch][0] = val_res; - if(sentStat[ch]) - { - sentStatusErr++; - } + sentTempValArr[ch] = ((uint16_t)(val_res) << 8); - sentSMstate[ch] = SM_SENT_SIG1_DATA1_STATE; - break; + sentSMstate[ch] = SM_SENT_SIG1_DATA2_STATE; + break; - case SM_SENT_SIG1_DATA1_STATE: - sentTempNibblArr[ch][0] = val_res; + case SM_SENT_SIG1_DATA2_STATE: + sentTempNibblArr[ch][1] = val_res; - sentTempValArr[ch] = ((uint16_t)(val_res) << 8); + sentTempValArr[ch] |= ((uint16_t)(val_res) << 4); - sentSMstate[ch] = SM_SENT_SIG1_DATA2_STATE; - break; + sentSMstate[ch] = SM_SENT_SIG1_DATA3_STATE; + break; - case SM_SENT_SIG1_DATA2_STATE: - sentTempNibblArr[ch][1] = val_res; + case SM_SENT_SIG1_DATA3_STATE: + sentTempNibblArr[ch][2] = val_res; - sentTempValArr[ch] |= ((uint16_t)(val_res) << 4); + sentTempValArr[ch] |= (val_res); - sentSMstate[ch] = SM_SENT_SIG1_DATA3_STATE; - break; + sentSMstate[ch] = SM_SENT_SIG2_DATA1_STATE; + break; - case SM_SENT_SIG1_DATA3_STATE: - sentTempNibblArr[ch][2] = val_res; + case SM_SENT_SIG2_DATA1_STATE: + sentTempNibblArr[ch][3] = val_res; - sentTempValArr[ch] |= (val_res); + sentRollCnt[ch] = ((uint8_t)(val_res) << 4); - sentSMstate[ch] = SM_SENT_SIG2_DATA1_STATE; - break; + sentSMstate[ch] = SM_SENT_SIG2_DATA2_STATE; + break; - case SM_SENT_SIG2_DATA1_STATE: - sentTempNibblArr[ch][3] = val_res; + case SM_SENT_SIG2_DATA2_STATE: + sentTempNibblArr[ch][4] = val_res; - sentRollCnt[ch] = ((uint8_t)(val_res) << 4); + sentRollCnt[ch] |= ((uint8_t)(val_res)); - sentSMstate[ch] = SM_SENT_SIG2_DATA2_STATE; - break; + sentSMstate[ch] = SM_SENT_SIG2_DATA3_STATE; + break; - case SM_SENT_SIG2_DATA2_STATE: - sentTempNibblArr[ch][4] = val_res; + case SM_SENT_SIG2_DATA3_STATE: + sentTempNibblArr[ch][5] = val_res; - sentRollCnt[ch] |= ((uint8_t)(val_res)); + sentSMstate[ch] = SM_SENT_CRC_STATE; + break; - sentSMstate[ch] = SM_SENT_SIG2_DATA3_STATE; - break; + case SM_SENT_CRC_STATE: + // Check msg number + if(sentRollCnt[ch] != (uint8_t)(sentRollCntPrev[ch] + 1)) + {// Msg lost + // Increment msg number err count + sentRollErrCnt++; + } - case SM_SENT_SIG2_DATA3_STATE: - sentTempNibblArr[ch][5] = val_res; + sentRollCntPrev[ch] = sentRollCnt[ch]; - sentSMstate[ch] = SM_SENT_CRC_STATE; - break; + // Check crc + if((uint8_t)(val_res) == sent_crc4(sentTempNibblArr[ch], SENT_MSG_PAYLOAD_SIZE)) + { + sentValArr[ch] = sentTempValArr[ch]; + } + else + { + // Increment crc err count + sentCrcErrCnt++; + } - case SM_SENT_CRC_STATE: - // Check msg number - if(sentRollCnt[ch] == (uint8_t)(sentRollCntPrev[ch] + 1)) - { - - } - else - { - // Increment nsg number err count - sentRollErrCnt++; - } - - sentRollCntPrev[ch] = sentRollCnt[ch]; - - // Check crc - sentCrc = ((uint8_t)(val_res)); - - if(sentCrc == sent_crc4(sentTempNibblArr[ch], 6)) - { - sentValArr[ch] = sentTempValArr[ch]; - } - else - { - // Increment crc err count - sentCrcErrCnt++; - } - - sentSMstate[ch] = SM_SENT_SYNC_STATE; - break; - } + sentSMstate[ch] = SM_SENT_SYNC_STATE; + break; + } + } } } -#define CRCSeed 0x05 - -static const uint8_t SENT_CRC4_tbl[16] = -{ - 0, 13, 7, 10, 14, 3, 9, 4, 1, 12, 6, 11, 15, 2, 8, 5 -}; - uint8_t sent_crc4(uint8_t* pdata, uint16_t ndata) { uint8_t crc; uint16_t i; - crc = 5; // Seed. + crc = SENT_CRC_SEED; // Seed. + for(i=0; i < ndata; i++) { crc = *pdata++ ^ SENT_CRC4_tbl[crc]; // Data[#1 to #6] } - //crc = 0 ^ crc4sent_tbl[crc]; // Post-process. - //return crc; // Return the CRC result. return SENT_CRC4_tbl[crc]; } diff --git a/SENT-box/firmware/sent.h b/SENT-box/firmware/sent.h index 4eda10e..8ffa9ff 100644 --- a/SENT-box/firmware/sent.h +++ b/SENT-box/firmware/sent.h @@ -1,13 +1,41 @@ /* * sent.h * - * Created on: 16 May 2022 + * Created on: 16 мая 2022 г. * Author: alexv */ #ifndef SENT_H_ #define SENT_H_ +#define SENT_CHANNELS_NUM 4 // Number of sent channels + +#define SENT_ICU_FREQ 400000 // 2.5 us + +#define SENT_SYNC_INTERVAL 44 // 56 ticks - 12 +#define SENT_OFFSET_INTERVAL 12 + +#define SENT_MIN_INTERVAL 12 +#define SENT_MAX_INTERVAL 15 + +#define SENT_CRC_SEED 0x05 + +#define SENT_MSG_PAYLOAD_SIZE 6 // Size of payload + +#define SENT_ERR_PERCENT 0 + +#define SENT_ICUD_CH1 ICUD4 +#define SENT_ICUD_CH2 ICUD3 +#define SENT_ICUD_CH3 ICUD1 + +enum +{ + SENT_CH1 = 0, + SENT_CH2, + SENT_CH3, + SENT_CH4, +}; + typedef enum { SM_SENT_INIT_STATE = 0, @@ -24,7 +52,6 @@ typedef enum void InitSent(); -uint16_t SentGetPeriodValue(void); uint16_t SENT_GetData(uint8_t ch); uint16_t SENT_GetRollErrCnt(void); diff --git a/SENT-box/firmware/uart.cpp b/SENT-box/firmware/uart.cpp index 037c891..fc4d650 100644 --- a/SENT-box/firmware/uart.cpp +++ b/SENT-box/firmware/uart.cpp @@ -34,7 +34,9 @@ static void UartThread(void*) while(true) { - size_t writeCount = chsnprintf(printBuffer, 200, "%04d %04d %04d %04d %04d %04d\r\n", SENT_GetData(0), SENT_GetData(1), SENT_GetData(2), SENT_GetData(3), SENT_GetRollErrCnt(), SENT_GetCrcErrCnt()); + size_t writeCount = chsnprintf(printBuffer, 200, "%04d %04d %04d %04d err %04d %04d %04d %04d\r\n", + SENT_GetData(SENT_CH1), SENT_GetData(SENT_CH2), SENT_GetData(SENT_CH3), SENT_GetData(SENT_CH4), + SENT_GetIntervalErrCnt(), SENT_GetSyncErrCnt(), SENT_GetRollErrCnt(), SENT_GetCrcErrCnt()); uartStartSend(&UARTD1, writeCount, printBuffer); chThdSleepMilliseconds(20);