From ceb1a164035c0fbb63f691768e839f4ecd23815c Mon Sep 17 00:00:00 2001 From: Pete Schwamb Date: Mon, 24 Apr 2017 15:44:22 -0500 Subject: [PATCH] initial code for tracking proximity --- Makefile | 9 +++++---- src/proximity.c | 35 +++++++++++++++++++++++++++++++++-- src/scheduler.c | 9 ++++----- src/sensor.c | 11 +++++++++++ src/sensor.h | 12 ++++++------ 5 files changed, 59 insertions(+), 17 deletions(-) diff --git a/Makefile b/Makefile index 4e95d5b..c82e92e 100644 --- a/Makefile +++ b/Makefile @@ -23,11 +23,11 @@ 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-DN00D34P # left #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 # Sparkfun board +SERIAL_PORT := /dev/cu.usbserial-FTZ86FTC # tag-connect +#SERIAL_PORT := /dev/cu.usbserial-DO00C2G2 # Sparkfun board ifeq ($(USE_RBC_MESH_SERIAL), "yes") SERIAL_STRING := "_serial" @@ -93,7 +93,8 @@ 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/scheduler.c src/proximity.c src/heartbeat.c src/battery.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 src/battery.c src/shoe_accel.c C_SOURCE_FILES += $(COMPONENTS)/libraries/timer/app_timer.c CXX_SOURCE_FILES += $(SIMBLEE_BASE)/libraries/SimbleeBLE/SimbleeBLE.cpp diff --git a/src/proximity.c b/src/proximity.c index 47968e6..02f77e4 100644 --- a/src/proximity.c +++ b/src/proximity.c @@ -1,5 +1,36 @@ +#include "proximity.h" +#include +static uint8_t rssi_sorted[MAX_PROXIMITY_TRACKING_COUNT]; +static uint8_t sensor_ids_sorted[MAX_PROXIMITY_TRACKING_COUNT]; -// Record RSSI of packets we hear from peers. +void proximity_add_entry(uint8_t sensor_id, uint8_t rssi) { + uint8_t i,j; + for (i=0; i < MAX_PROXIMITY_TRACKING_COUNT; i++) { + if (rssi < rssi_sorted[0] || rssi_sorted[0] == 0) { + j = MAX_PROXIMITY_TRACKING_COUNT-1; + while (j > i) { + rssi_sorted[j] = rssi_sorted[j-1]; + sensor_ids_sorted[j] = sensor_ids_sorted[j-1]; + j--; + } + rssi_sorted[i] = rssi; + sensor_ids_sorted[i] = sensor_id; + } + } +} -// use tc_packet_peek_cb_set ? +void proximity_values_reset() { + memset(rssi_sorted, 0, sizeof(uint8_t) * MAX_PROXIMITY_TRACKING_COUNT); + memset(sensor_ids_sorted, 0, sizeof(uint8_t) * MAX_PROXIMITY_TRACKING_COUNT); +} + +uint8_t proximity_get_strongest_signals(uint8_t *sensor_ids, int8_t *rssi_values, uint8_t output_array_size) { + uint8_t n = 0; + while(n 0) { + n++; + } + memcpy(sensor_ids, sensor_ids_sorted, sizeof(uint8_t) * n); + memcpy(rssi_values, rssi_sorted, sizeof(uint8_t) * n); + return n; +} diff --git a/src/scheduler.c b/src/scheduler.c index 8d80aad..a808d01 100644 --- a/src/scheduler.c +++ b/src/scheduler.c @@ -66,11 +66,10 @@ static void periodic_timer_cb(void * p_context) 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; + + // This gives some sensors time to come online before data is collected. + sensor_warmup_event(); + if (delay_ticks > 5) { if (app_timer_start(m_offset_timer_ID, delay_ticks, NULL) != NRF_SUCCESS) { toggle_led(LED_RED); diff --git a/src/sensor.c b/src/sensor.c index 1321e5f..ba04709 100644 --- a/src/sensor.c +++ b/src/sensor.c @@ -5,19 +5,30 @@ #include "scheduler.h" #include "battery.h" #include "handles.h" +#include "shoe_accel.h" #include static sensor_value_t m_value; void sensor_init() { + shoe_accel_init(); + uint32_t error_code; error_code = rbc_mesh_value_enable(SENSOR_HANDLE); APP_ERROR_CHECK(error_code); } +void sensor_warmup_event() { + enable_shoe_accel(); +} + void gather_sensor_data() { m_value.uptime = get_uptime(); m_value.battery = get_battery_adc(); + + read_shoe_accel(&m_value.accel_x, &m_value.accel_y, &m_value.accel_z); + + disable_shoe_accel(); } void report_sensor_data() { diff --git a/src/sensor.h b/src/sensor.h index a451eef..533cf77 100644 --- a/src/sensor.h +++ b/src/sensor.h @@ -5,22 +5,22 @@ #include #include "toolchain.h" -#define MAX_PROXIMITY_TRACKING_COUNT 5 +#define MAX_SENSOR_ID = 64 typedef __packed_armcc struct { uint8_t proximity_ids[MAX_PROXIMITY_TRACKING_COUNT]; - uint8_t proximity_rssis[MAX_PROXIMITY_TRACKING_COUNT]; + uint8_t proximity_rss[MAX_PROXIMITY_TRACKING_COUNT]; uint8_t battery; - uint8_t accel_x; - uint8_t accel_y; - uint8_t accel_z; + int8_t accel_x; + int8_t accel_y; + int8_t accel_z; uint8_t status; uint32_t uptime; } __packed_gcc sensor_value_t; void sensor_init(); - +void sensor_warmup_event(); void report_sensor_data(); #endif