diff --git a/firmware/hw_layer/drivers/sent/sent.cpp b/firmware/hw_layer/drivers/sent/sent.cpp index ff47131de5..541917a789 100644 --- a/firmware/hw_layer/drivers/sent/sent.cpp +++ b/firmware/hw_layer/drivers/sent/sent.cpp @@ -291,9 +291,18 @@ int sent_channel::FastChannelDecoder(uint32_t clocks) { return 0; } -int sent_channel::Decoder(uint32_t clocks) { +int sent_channel::Decoder(uint32_t clocks, uint8_t flags) { int ret; + #if SENT_STATISTIC_COUNTERS + if (flags & SENT_FLAG_HW_OVERFLOW) { + statistic.hwOverflowCnt++; + } + #endif + + /* TODO: handle flags */ + (void)flags; + ret = FastChannelDecoder(clocks); if (ret > 0) { /* valid packet received, can process slow channels */ @@ -569,6 +578,8 @@ void sent_channel::Info() { } #if SENT_STATISTIC_COUNTERS + efiPrintf("HW overflows %lu\n", statistic.hwOverflowCnt); + efiPrintf("Pause pulses %lu\n", statistic.PauseCnt); efiPrintf("Restarts %lu", statistic.RestartCnt); efiPrintf("Interval errors %lu short, %lu long", statistic.ShortIntervalErr, statistic.LongIntervalErr); @@ -590,9 +601,9 @@ static MAILBOX_DECL(sent_mb, sent_mb_buffer, SENT_MB_SIZE); static THD_WORKING_AREA(waSentDecoderThread, 256); -void SENT_ISR_Handler(uint8_t channel, uint16_t clocks) { +void SENT_ISR_Handler(uint8_t channel, uint16_t clocks, uint8_t flags) { /* encode to fit msg_t */ - msg_t msg = (channel << 16) | clocks; + msg_t msg = (flags << 24) | (channel << 16) | clocks; /* called from ISR */ chSysLockFromISR(); @@ -610,11 +621,12 @@ static void SentDecoderThread(void*) { if (ret == MSG_OK) { uint16_t tick = msg & 0xffff; uint8_t n = (msg >> 16) & 0xff; + uint8_t flags = (msg >> 24) & 0xff; if (n < SENT_CHANNELS_NUM) { sent_channel &channel = channels[n]; - if (channel.Decoder(tick) > 0) { + if (channel.Decoder(tick, flags) > 0) { /* report only for first channel */ if (n == 0) { uint16_t sig0, sig1; diff --git a/firmware/hw_layer/drivers/sent/sent.h b/firmware/hw_layer/drivers/sent/sent.h index ca3173a57f..526cd0f625 100644 --- a/firmware/hw_layer/drivers/sent/sent.h +++ b/firmware/hw_layer/drivers/sent/sent.h @@ -14,8 +14,10 @@ /* SENT decoder init */ void initSent(); +#define SENT_FLAG_HW_OVERFLOW (1 << 0) + /* ISR hook */ -void SENT_ISR_Handler(uint8_t ch, uint16_t val_res); +void SENT_ISR_Handler(uint8_t channels, uint16_t clocks, uint8_t flags); /* Stop/Start for config update */ void startSent(); diff --git a/firmware/hw_layer/drivers/sent/sent_hw_icu.cpp b/firmware/hw_layer/drivers/sent/sent_hw_icu.cpp index 398a402b7b..dbbfe1749c 100644 --- a/firmware/hw_layer/drivers/sent/sent_hw_icu.cpp +++ b/firmware/hw_layer/drivers/sent/sent_hw_icu.cpp @@ -22,32 +22,46 @@ /* This SENT HW driver is based on ChibiOS ICU driver */ #if (HAL_USE_ICU == TRUE) +/* TODO: do we care about scaling abstract timer ticks to some time base? */ /* TODO: get at runtime */ /* Max timer clock for most timers on STM32 is CPU clock / 2 */ #define SENT_TIMER_CLOCK_DIV 2 #define SENT_ICU_FREQ (CORE_CLOCK / SENT_TIMER_CLOCK_DIV) // == CPU freq / 2 static uint16_t lastPulse[SENT_INPUT_COUNT]; +static bool overcapture[SENT_INPUT_COUNT]; static void icuperiodcb(ICUDriver *icup, size_t index) { + uint16_t clocks; + uint8_t flags = 0; const ICUConfig *icucfg = icup->config; if ((icucfg->channel == ICU_CHANNEL_1) || (icucfg->channel == ICU_CHANNEL_2)) { /* channel 1 and channel 2 supports period measurements */ - SENT_ISR_Handler(index, icuGetPeriodX(icup) * SENT_TIMER_CLOCK_DIV); + clocks = icuGetPeriodX(icup); } else { /* this is freerunnig timer and we need to calculate period using just captured timer value and previous one */ /* TODO: support 32 bit timers too? */ uint16_t val = icuGetWidthX(icup); /* can overflow */ - uint16_t clocks = val - lastPulse[index]; - - SENT_ISR_Handler(index, clocks /* * SENT_TIMER_CLOCK_DIV */); + clocks = val - lastPulse[index]; lastPulse[index] = val; } + + if (overcapture[index]) { + flags |= SENT_FLAG_HW_OVERFLOW; + overcapture[index] = false; + } + + SENT_ISR_Handler(index, clocks, flags); +} + +static void icuovercapture(ICUDriver *icup, size_t index) +{ + overcapture[index] = true; } /* ICU callbacks */ @@ -56,6 +70,11 @@ static void icuperiodcb_in1(ICUDriver *icup) icuperiodcb(icup, 0); } +static void icuovercapture_in1(ICUDriver *icup) +{ + icuovercapture(icup, 0); +} + /* ICU configs */ static ICUConfig icucfg[SENT_INPUT_COUNT] = { @@ -68,6 +87,7 @@ static ICUConfig icucfg[SENT_INPUT_COUNT] = .channel = ICU_CHANNEL_1, /* will be overwriten on startSent() */ .dier = 0U, .arr = 0xFFFFFFFFU, + .overcapture_cb = icuovercapture_in1, } }; diff --git a/firmware/hw_layer/drivers/sent/sent_logic.h b/firmware/hw_layer/drivers/sent/sent_logic.h index ad94d24f1f..1c652a33de 100644 --- a/firmware/hw_layer/drivers/sent/sent_logic.h +++ b/firmware/hw_layer/drivers/sent/sent_logic.h @@ -30,6 +30,8 @@ typedef enum } SENT_STATE_enum; struct sent_channel_stat { + uint32_t hwOverflowCnt; + uint32_t ShortIntervalErr; uint32_t LongIntervalErr; uint32_t SyncErr; @@ -107,7 +109,7 @@ public: #endif // SENT_STATISTIC_COUNTERS /* Decoder */ - int Decoder(uint32_t clocks); + int Decoder(uint32_t clocks, uint8_t flags = 0); /* Get last raw message */ int GetMsg(uint32_t* rx);