From 81fde7ba86e76e9914182dcc15f8d72aaccab6e8 Mon Sep 17 00:00:00 2001 From: Pete Schwamb Date: Fri, 21 Apr 2017 15:34:14 -0500 Subject: [PATCH] Time sync working --- Makefile | 6 +- pyaci/aci/AciEvent.py | 2 +- src/heartbeat.c | 13 +- src/heartbeat.h | 5 +- src/main.c | 18 +-- src/scheduler.c | 204 +++++++++++++++++++++++++++++++ src/{time_sync.h => scheduler.h} | 24 ++-- src/sensor.c | 2 +- src/sensor.h | 2 +- src/time_sync.c | 79 ------------ 10 files changed, 238 insertions(+), 117 deletions(-) create mode 100644 src/scheduler.c rename src/{time_sync.h => scheduler.h} (53%) delete mode 100644 src/time_sync.c diff --git a/Makefile b/Makefile index b48623c..7ac42c5 100644 --- a/Makefile +++ b/Makefile @@ -24,10 +24,10 @@ RBC_MESH := rbc_mesh LINKER_SCRIPT := $(SIMBLEE_BASE)/variants/Simblee/linker_scripts/gcc/Simblee.ld RFD_LOADER := $(SIMBLEE_BASE)/RFDLoader_osx #SERIAL_PORT := /dev/cu.usbserial-DN00D34P # left -#SERIAL_PORT := /dev/cu.usbserial-DN00CSZ7 # right +SERIAL_PORT := /dev/cu.usbserial-DN00CSZ7 # right #SERIAL_PORT := /dev/cu.usbserial-A105RB12 #SERIAL_PORT := /dev/cu.usbserial-FTZ86FTC -SERIAL_PORT := /dev/cu.usbserial-DO00C2G2 +#SERIAL_PORT := /dev/cu.usbserial-DO00C2G2 ifeq ($(USE_RBC_MESH_SERIAL), "yes") SERIAL_STRING := "_serial" @@ -93,7 +93,7 @@ remduplicates = $(strip $(if $1,$(firstword $1) $(call remduplicates,$(filter-ou # source common to all targets -C_SOURCE_FILES += src/main.c src/leds.c src/config.c src/sensor.c src/app_cmd.c src/time_sync.c src/proximity.c src/heartbeat.c +C_SOURCE_FILES += src/main.c src/leds.c src/config.c src/sensor.c src/app_cmd.c src/scheduler.c src/proximity.c src/heartbeat.c C_SOURCE_FILES += $(COMPONENTS)/libraries/timer/app_timer.c CXX_SOURCE_FILES += $(SIMBLEE_BASE)/libraries/SimbleeBLE/SimbleeBLE.cpp diff --git a/pyaci/aci/AciEvent.py b/pyaci/aci/AciEvent.py index 3bd1ce6..5b0fdfd 100644 --- a/pyaci/aci/AciEvent.py +++ b/pyaci/aci/AciEvent.py @@ -99,7 +99,7 @@ def __init__(self,pkt): if self.Len < 3: logging.error("Invalid length for %s event: %s", self.__class__.__name__, str(pkt)) else: - self.ValueHandle = (pkt[2] << 8) + pkt[3] + self.ValueHandle = (pkt[3] << 8) + pkt[2] self.Data = pkt[4:] def __repr__(self): diff --git a/src/heartbeat.c b/src/heartbeat.c index ccfae92..31ca337 100644 --- a/src/heartbeat.c +++ b/src/heartbeat.c @@ -2,8 +2,8 @@ #include "mesh_packet.h" #include "transport_control.h" #include "config.h" -#include "time_sync.h" #include "leds.h" +#include "scheduler.h" static tc_tx_config_t m_tx_config; @@ -14,7 +14,7 @@ void heartbeat_init() { m_tx_config.tx_power = RBC_MESH_TXPOWER_0dBm; } -void send_heartbeat_packet() { +void send_heartbeat_packet(uint8_t sensor_id, uint32_t epoch_seconds, uint16_t epoch_ms, uint16_t clock_version) { // Send out time sync packet mesh_packet_t *p_packet; @@ -34,9 +34,10 @@ void send_heartbeat_packet() { p_adv_data->adv_data_type = HEARTBEAT_ADV_DATA_TYPE; // Normal mesh packets are MESH_ADV_DATA_TYPE (0x16) heartbeat_ad_t* p_heartbeat_ad = (heartbeat_ad_t*) &p_adv_data->data[0]; - p_heartbeat_ad->sensor_id = get_sensor_id(); - p_heartbeat_ad->epoch_time = get_clock_time(); - p_heartbeat_ad->clock_version = get_clock_version(); + p_heartbeat_ad->sensor_id = sensor_id; + p_heartbeat_ad->epoch_seconds = epoch_seconds; + p_heartbeat_ad->epoch_ms = epoch_ms; + p_heartbeat_ad->clock_version = clock_version; if (tc_tx(p_packet, &m_tx_config) != NRF_SUCCESS) { toggle_led(LED_RED); @@ -47,5 +48,5 @@ void send_heartbeat_packet() { } void received_heartbeat(heartbeat_ad_t *p_heartbeat_ad) { - set_clock_time(p_heartbeat_ad->epoch_time, 0, CLOCK_SOURCE_RF, p_heartbeat_ad->clock_version); + set_clock_time(p_heartbeat_ad->epoch_seconds, p_heartbeat_ad->epoch_ms, CLOCK_SOURCE_RF, p_heartbeat_ad->clock_version); } diff --git a/src/heartbeat.h b/src/heartbeat.h index 5cf78ed..65abfd0 100644 --- a/src/heartbeat.h +++ b/src/heartbeat.h @@ -10,13 +10,14 @@ typedef __packed_armcc struct { uint8_t sensor_id; - uint32_t epoch_time; + uint32_t epoch_seconds; + uint16_t epoch_ms; uint16_t clock_version; } __packed_gcc heartbeat_ad_t; void heartbeat_init(); -void send_heartbeat_packet(); +void send_heartbeat_packet(uint8_t sensor_id, uint32_t epoch_seconds, uint16_t current_ms, uint16_t clock_version); void received_heartbeat(heartbeat_ad_t *p_heartbeat_ad); #endif // HEARTBEAT_H diff --git a/src/main.c b/src/main.c index ff035ae..cbdd9d9 100644 --- a/src/main.c +++ b/src/main.c @@ -13,7 +13,7 @@ #include "config.h" #include "sensor.h" #include "transport_control.h" -#include "time_sync.h" +#include "scheduler.h" #include "heartbeat.h" #include #include @@ -110,23 +110,10 @@ void clock_initialization() static void packet_peek_cb(rbc_mesh_packet_peek_params_t *params) { if (params->packet_type == BLE_PACKET_TYPE_ADV_NONCONN_IND && params->p_payload[1] == HEARTBEAT_ADV_DATA_TYPE) { - toggle_led(LED_BLUE); received_heartbeat((heartbeat_ad_t*)¶ms->p_payload[2]); } } -// Main timer callbacks; from the synchronized clock -void main_timer_cb() { - if (get_clock_time() % 10 == 0) { - toggle_led(LED_GREEN); - send_heartbeat_packet(); - } -} - -void offset_timer_cb() { - sensor_update(); -} - int main(void) { //clock_initialization(); @@ -174,7 +161,7 @@ int main(void) tc_radio_params_set(MESH_ACCESS_ADDR, app_config.mesh_channel); } - time_sync_init(); // Initializes, but does not start, timer + scheduler_init(); // Initializes, but does not start, timer heartbeat_init(); // Inits structures for sending heartbeat /* Initialize serial ACI */ @@ -190,7 +177,6 @@ int main(void) /* Enable our handle */ if (app_config.sensor_id > 0) { sensor_init(); - toggle_led(LED_GREEN); } /* Main event loop */ diff --git a/src/scheduler.c b/src/scheduler.c new file mode 100644 index 0000000..70f2a02 --- /dev/null +++ b/src/scheduler.c @@ -0,0 +1,204 @@ + +#include "scheduler.h" +#include "leds.h" +#include "sensor.h" +#include "app_timer.h" +#include "config.h" +#include "rand.h" +#include "heartbeat.h" +#include "sensor.h" +#include "rbc_mesh.h" + +// Timer settings +#define APP_TIMER_PRESCALER 15 // divisor value - 1 +#define TICKS_PER_SECOND 2048 // f / (APP_TIMER_PRESCALER+1) +#define TICKS_PER_100ms 205 // f / (APP_TIMER_PRESCALER+1) / 10 +#define APP_TIMER_MAX_TIMERS 3 +#define APP_TIMER_OP_QUEUE_SIZE 4 + +static int32_t m_current_time; +static int16_t m_clock_version = 0; +static app_timer_id_t m_clock_sync_timer_ID; +static app_timer_id_t m_offset_timer_ID; +static app_timer_id_t m_periodic_timer_ID; +static scheduler_state_t m_scheduler_state; +static prng_t m_rand; +static uint32_t m_clock_second_start_counter_value; + +#define MS_TO_TICKS(MS) ((TICKS_PER_100ms * (MS)) / 100) +#define TICKS_TO_MS(TICKS) (100 * (TICKS) / TICKS_PER_100ms) + +static void offset_timer_cb(void * p_context); +static void delay_to_heartbeat(); + +#define DEBUG_REGISTER_SIZE (16) +#define DEBUG_REGISTER_HANDLE (0x1234) +static uint8_t debug_counter; +static uint8_t debug_register[DEBUG_REGISTER_SIZE]; + +static void add_value_to_debug_register(uint8_t value) { + debug_register[debug_counter % DEBUG_REGISTER_SIZE] = value; + debug_counter++; +} +static void report_debug_register() { + rbc_mesh_value_set(DEBUG_REGISTER_HANDLE, debug_register, DEBUG_REGISTER_SIZE); +} + +static void periodic_timer_cb(void * p_context) +{ + + led_config(LED_GREEN, 1); + app_timer_cnt_get(&m_clock_second_start_counter_value); + m_scheduler_state = SCHEDULER_STATE_BEFORE_HB; + m_current_time += 1; + //rbc_mesh_start(); + + if (m_current_time % 10 == 0) { + //report_debug_register(); + } + + // Delay to heartbeat + delay_to_heartbeat(); +} + +static void delay_to_heartbeat() { + uint16_t random_tx_delay = ((rand_prng_get(&m_rand) & 0x3ff) * HEARTBEAT_WINDOW_MS) / 0x3ff; + int32_t delay_ticks = MS_TO_TICKS(MAX_EXPECTED_CLOCK_SKEW_MS + random_tx_delay); + if (random_tx_delay > 1000) { + toggle_led(LED_RED); + } + add_value_to_debug_register(delay_ticks & 0xff); + delay_ticks = 5 + (debug_counter * 2) % 205; + if (delay_ticks > 5) { + if (app_timer_start(m_offset_timer_ID, delay_ticks, NULL) != NRF_SUCCESS) { + toggle_led(LED_RED); + } + } else { + toggle_led(LED_RED); + offset_timer_cb(NULL); + } +} + +static void do_heartbeat() { + led_config(LED_BLUE, 1); + uint32_t current_counter; + app_timer_cnt_get(¤t_counter); + // Modulo wraparound makes this ok + uint32_t elapsed_ticks_since_second_start = current_counter - m_clock_second_start_counter_value; + send_heartbeat_packet(get_sensor_id(), m_current_time, TICKS_TO_MS(elapsed_ticks_since_second_start), m_clock_version); +} + +static void delay_to_reporting() { + // We do a fixed delay here, even though the last delay (delay_to_heartbeat) + // was a random interval, as we want the reporting to start with some randomness + // as well, to avoid all nodes starting the mesh sync at the same time + int32_t delay_ticks = MS_TO_TICKS(HEARTBEAT_WINDOW_MS); + if (delay_ticks > 5) { + app_timer_start(m_offset_timer_ID, delay_ticks, NULL); + } else { + offset_timer_cb(NULL); + } +} + +static void do_reporting() { + led_config(LED_BLUE, 0); + report_sensor_data(); +} + +static void delay_to_sleep() { + uint32_t current_counter; + app_timer_cnt_get(¤t_counter); + uint32_t elapsed_ticks_since_second_start = current_counter - m_clock_second_start_counter_value; + int32_t delay_ticks = MS_TO_TICKS(TOTAL_RADIO_WINDOW_MS) - elapsed_ticks_since_second_start; + if (delay_ticks > 5) { + app_timer_start(m_offset_timer_ID, delay_ticks, NULL); + } else { + offset_timer_cb(NULL); + } +} + +static void do_sleep() { + //rbc_mesh_stop(); + led_config(LED_GREEN, 0); +} + +static void offset_timer_cb(void * p_context) { + app_timer_stop(m_offset_timer_ID); + + switch (m_scheduler_state) { + case SCHEDULER_STATE_BEFORE_HB: + do_heartbeat(); + m_scheduler_state = SCHEDULER_STATE_AFTER_HB; + delay_to_reporting(); + break; + case SCHEDULER_STATE_AFTER_HB: + do_reporting(); + m_scheduler_state = SCHEDULER_STATE_REPORTING; + delay_to_sleep(); + break; + case SCHEDULER_STATE_REPORTING: + do_sleep(); + m_scheduler_state = SCHEDULER_STATE_SLEEP; + // periodic timer will wake us next time + break; + default: + toggle_led(LED_RED); + break; + } +} + +static void start_periodic_timer() { + app_timer_start(m_periodic_timer_ID, TICKS_PER_SECOND, NULL); +} + +static void clock_sync_cb(void * p_context) { + m_current_time += 1; + start_periodic_timer(); +} + +static void start_timer(uint16_t start_delay) { + int32_t start_delay_ticks = (start_delay * (TICKS_PER_SECOND/1000.0)); + if (start_delay_ticks > 5) { + app_timer_start(m_clock_sync_timer_ID, start_delay_ticks, NULL); + } else { + start_periodic_timer(); + } +} + +void scheduler_init() { + rand_prng_seed(&m_rand); + m_scheduler_state = SCHEDULER_STATE_STOPPED; + APP_TIMER_INIT(APP_TIMER_PRESCALER, APP_TIMER_MAX_TIMERS, APP_TIMER_OP_QUEUE_SIZE, false); + app_timer_create(&m_periodic_timer_ID, APP_TIMER_MODE_REPEATED, periodic_timer_cb); + app_timer_create(&m_clock_sync_timer_ID, APP_TIMER_MODE_SINGLE_SHOT, clock_sync_cb); + app_timer_create(&m_offset_timer_ID, APP_TIMER_MODE_SINGLE_SHOT, offset_timer_cb); +} + +void set_clock_time(int32_t epoch, uint16_t ms, clock_source_t clock_source, int16_t clock_version) { + + if (clock_source == CLOCK_SOURCE_RF) { + // modulo math to handle wraparound + uint16_t version_delta = clock_version - m_clock_version; + if (version_delta > 0 && version_delta < 0xff00) { + m_clock_version = clock_version; + } else { + // Older or same clock version + return; + } + } else if (clock_source == CLOCK_SOURCE_SERIAL) { + m_clock_version++; + } + m_current_time = epoch; + uint16_t start_delay = (1000 - ms) % 1000; + app_timer_stop(m_periodic_timer_ID); + app_timer_stop(m_clock_sync_timer_ID); + start_timer(start_delay); +} + +int32_t get_clock_time() { + return m_current_time; +} + +int16_t get_clock_version() { + return m_clock_version; +} diff --git a/src/time_sync.h b/src/scheduler.h similarity index 53% rename from src/time_sync.h rename to src/scheduler.h index 7f5d429..538dfad 100644 --- a/src/time_sync.h +++ b/src/scheduler.h @@ -1,5 +1,5 @@ -#ifndef __TIME_SYNC_H_ -#define __TIME_SYNC_H_ +#ifndef __SCHEDULER_H_ +#define __SCHEDULER_H_ #include #include "toolchain.h" @@ -10,7 +10,19 @@ typedef enum { CLOCK_SOURCE_SERIAL, } clock_source_t; -void time_sync_init(); +typedef enum { + SCHEDULER_STATE_STOPPED, + SCHEDULER_STATE_SLEEP, + SCHEDULER_STATE_BEFORE_HB, + SCHEDULER_STATE_AFTER_HB, + SCHEDULER_STATE_REPORTING, +} scheduler_state_t; + +#define MAX_EXPECTED_CLOCK_SKEW_MS (10) +#define HEARTBEAT_WINDOW_MS (100) +#define TOTAL_RADIO_WINDOW_MS (400) + +void scheduler_init(); void set_clock_time(int32_t epoch, uint16_t ms, clock_source_t clock_source, int16_t clock_version); // Returns unix epoch @@ -19,8 +31,4 @@ int32_t get_clock_time(); // in the heartbeat message int16_t get_clock_version(); -// Callbacks -void main_timer_cb(); -void offset_timer_cb(); - -#endif //__TIME_SYNC_H_ +#endif //__SCHEDULER_H_ diff --git a/src/sensor.c b/src/sensor.c index 5a9cd8f..47a2364 100644 --- a/src/sensor.c +++ b/src/sensor.c @@ -14,7 +14,7 @@ void sensor_init() { APP_ERROR_CHECK(error_code); } -void sensor_update() { +void report_sensor_data() { uint32_t error_code; error_code = rbc_mesh_value_set(SENSOR_HANDLE, (uint8_t*)&m_value, sizeof(sensor_value_t)); APP_ERROR_CHECK(error_code); diff --git a/src/sensor.h b/src/sensor.h index d8be230..a451eef 100644 --- a/src/sensor.h +++ b/src/sensor.h @@ -21,6 +21,6 @@ typedef __packed_armcc struct void sensor_init(); -void sensor_update(); +void report_sensor_data(); #endif diff --git a/src/time_sync.c b/src/time_sync.c deleted file mode 100644 index 0a19bdc..0000000 --- a/src/time_sync.c +++ /dev/null @@ -1,79 +0,0 @@ - -#include "time_sync.h" -#include "leds.h" -#include "sensor.h" -#include "app_timer.h" -#include "config.h" - -// Timer settings -#define APP_TIMER_PRESCALER 31 // divisor value - 1 -#define TICKS_PER_SECOND 1024 // f / (APP_TIMER_PRESCALER+1) -#define APP_TIMER_MAX_TIMERS 2 -#define APP_TIMER_OP_QUEUE_SIZE 3 - -static int32_t m_current_time; -static int16_t m_clock_version = 0; -static app_timer_id_t m_alignment_timer_ID; -static app_timer_id_t m_measurement_timer_ID; - -static void measurement_timer_cb(void * p_context) -{ - m_current_time += 1; - main_timer_cb(); - - // Todo; this should kick off some amount of time later, after - // all heartbeats have been observed and collected - offset_timer_cb(); -} - -static void start_measurement_timer() { - app_timer_start(m_measurement_timer_ID, TICKS_PER_SECOND, NULL); -} - -static void timer_aligned_cb(void * p_context) { - m_current_time += 1; - start_measurement_timer(); -} - -static void start_timer(uint16_t start_delay) { - int32_t start_delay_ticks = (start_delay * (TICKS_PER_SECOND/1000.0)); - if (start_delay_ticks > 5) { - app_timer_start(m_alignment_timer_ID, start_delay_ticks, NULL); - } else { - start_measurement_timer(); - } -} - -void time_sync_init() { - APP_TIMER_INIT(APP_TIMER_PRESCALER, APP_TIMER_MAX_TIMERS, APP_TIMER_OP_QUEUE_SIZE, false); - app_timer_create(&m_measurement_timer_ID, APP_TIMER_MODE_REPEATED, measurement_timer_cb); - app_timer_create(&m_alignment_timer_ID, APP_TIMER_MODE_SINGLE_SHOT, timer_aligned_cb); -} - -void set_clock_time(int32_t epoch, uint16_t ms, clock_source_t clock_source, int16_t clock_version) { - - if (clock_source == CLOCK_SOURCE_RF) { - // modulo math to handle wraparound - uint16_t version_delta = clock_version - m_clock_version; - if (version_delta > 0 && version_delta < 0xff00) { - m_clock_version = clock_version; - } else { - // Older or same clock version - return; - } - } else if (clock_source == CLOCK_SOURCE_SERIAL) { - m_clock_version++; - } - m_current_time = epoch; - uint16_t start_delay = (1000 - ms) % 1000; - app_timer_stop_all(); - start_timer(start_delay); -} - -int32_t get_clock_time() { - return m_current_time; -} - -int16_t get_clock_version() { - return m_clock_version; -}