3rd channel works now, also refactoring
This commit is contained in:
rusefillc
parent
e4026cc8c1
commit
16d610ad43
|
|
@ -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];
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue