/* * SPDX-License-Identifier: Apache-2.0 */ #include "beat_sync.h" #include "esp_timer.h" namespace { constexpr uint8_t kMagic = 0x4C; constexpr uint8_t kBeat = 0x25; constexpr uint32_t kDefaultPeriodMs = 500; constexpr uint32_t kMinPeriodMs = 30; constexpr uint32_t kMaxPeriodMs = 4000; constexpr int kLostPeriods = 4; // no beat for this many periods: the controller has stopped volatile uint16_t s_last_idx; volatile uint32_t s_period_ms; volatile int64_t s_last_us; volatile bool s_have; } // namespace void beat_on_frame(const uint8_t *data, size_t len) { if (len != 8 || data[0] != kMagic || data[1] != kBeat) { return; } uint16_t idx = (uint16_t)(data[3] | (data[4] << 8)); uint32_t period = (uint32_t)(data[5] | (data[6] << 8)); if (s_have && idx == s_last_idx) { return; // the second copy of a beat already taken } if (period < kMinPeriodMs || period > kMaxPeriodMs) { return; } s_last_idx = idx; s_period_ms = period; s_last_us = esp_timer_get_time(); s_have = true; } uint32_t beat_period_ms(void) { if (!s_have) { return kDefaultPeriodMs; } int64_t age_us = esp_timer_get_time() - s_last_us; if (age_us > (int64_t)s_period_ms * kLostPeriods * 1000) { return kDefaultPeriodMs; } return s_period_ms; }