From d65fa5225239adfe949663801bd7d366a0f97b33 Mon Sep 17 00:00:00 2001 From: voroshkov Date: Mon, 7 Jan 2019 12:34:18 +0300 Subject: [PATCH] fix detection algorithm (don't use PROGMEM because it requires special reading commands) --- .../chorus_rf_laptimer/chorus_rf_laptimer.ino | 20 +++++++++++----- .../chorus_rf_laptimer/lapDetectionRoutines.h | 24 +++++++++++++------ .../mainDetectionAlgorithm.h | 5 ---- Arduino/chorus_rf_laptimer/sounds.h | 12 +++++++++- 4 files changed, 42 insertions(+), 19 deletions(-) diff --git a/Arduino/chorus_rf_laptimer/chorus_rf_laptimer.ino b/Arduino/chorus_rf_laptimer/chorus_rf_laptimer.ino index 7bd4bed..ed6ea1e 100644 --- a/Arduino/chorus_rf_laptimer/chorus_rf_laptimer.ino +++ b/Arduino/chorus_rf_laptimer/chorus_rf_laptimer.ino @@ -59,7 +59,11 @@ uint8_t MODULE_ID_HEX = '0'; #define BAUDRATE 115200 -const uint16_t musicNotes[] PROGMEM = { 523, 587, 659, 698, 784, 880, 988, 1046 }; +const uint16_t musicNotes[] = { + 523, 587, 659, 698, 784, 880, 988, + 523*2, 587*2, 659*2, 698*2, 784*2, 880*2, 988*2, + 523*3, 587*3, 659*3, 698*3, 784*3, 880*3, 988*3, +}; // number of analog rssi reads to average for the current check. // single analog read with FASTADC defined (see below) takes ~20us on 16MHz arduino @@ -171,15 +175,19 @@ uint16_t rssi2; // rssi measured using second (deeper/slower) filter uint16_t rssi3; // rssi measured using third (even deeper/slower) filter that is expected to produce a smooth curve uint16_t rssiForThresholdSetup; // special rssi for threshold setup (slooow filter) -#define PROXIMITY_STEPS 10 +#define PROXIMITY_STEPS 20 // starts tracking proximity rssi from threshold decreased by this amount bool isApproaching = false; -const uint32_t proximityTimesArray[PROXIMITY_STEPS] PROGMEM = {20, 50, 100, 200, 500, 700, 1000, 1200, 1500, 2000}; -uint16_t currentProximityIndex; +const uint16_t proximityTimesArray[PROXIMITY_STEPS] = { + 20, 50, 100, 200, 500, 700, 1000, 1200, 1500, 2000, + 2500, 2500, 2500, 2500, 2500, 2500, 3000, 3000, 3000, 3000 +}; +uint8_t currentProximityIndex; uint32_t currentProximityIndexTime; //lap detection variables bool isFirstThresholdCrossed; bool didLeaveDeviceAreaThisLap; +bool isLapDetectionTimeoutExpired; uint16_t upperSecondLevelRssiThreshold; uint16_t lowerSecondLevelThreshold; uint16_t minDeepRssi; @@ -202,7 +210,7 @@ uint32_t smoothlyFilteredRssiMultiplied; #define DEFAULT_MAX_RSSI_SEARCH_DELAY 3000 // time to watch for max value after crossing a threshold #define ALLOWED_LAP_DETECTION_TIMEOUT 1000 // time allowed for lap detection after first threshold is crossed -#define RELIABLE_RSSI_DETECTION_SUBTRACT 4 // decrease found upperSecondLevelRssiThreshold by this amount to track initial threshold cross event on all laps after 1st +// #define RELIABLE_RSSI_DETECTION_SUBTRACT 4 // decrease found upperSecondLevelRssiThreshold by this amount to track initial threshold cross event on all laps after 1st #define SECOND_LEVEL_RSSI_DETECTION_ADJUSTMENT 2 // decrease found maxDeepRssi by this amount to make next lap detection more reliable #define EDGE_RSSI_ADJUSTMENT 10 // decrease found maximum (and increase minimum) by this value after each lap to better find a new one @@ -753,7 +761,7 @@ void readSerialDataChunk () { uint8_t availBytes = Serial.available(); if (availBytes) { if (availBytes > READ_BUFFER_SIZE) { - digitalHigh(ledPin); + // digitalHigh(ledPin); } uint8_t freeBufBytes = READ_BUFFER_SIZE - readBufFilledBytes; diff --git a/Arduino/chorus_rf_laptimer/lapDetectionRoutines.h b/Arduino/chorus_rf_laptimer/lapDetectionRoutines.h index 5b39341..ed62178 100644 --- a/Arduino/chorus_rf_laptimer/lapDetectionRoutines.h +++ b/Arduino/chorus_rf_laptimer/lapDetectionRoutines.h @@ -62,13 +62,23 @@ bool checkIsMaxRssiDetectionTimeoutNotExpired() { } bool checkIsLapDetectionTimeoutExpired() { + if (isLapDetectionTimeoutExpired) return true; + uint32_t diff = now - lastMilliseconds; - return diff > minLapTime * 1000; + isLapDetectionTimeoutExpired = diff > minLapTime * 1000; + return isLapDetectionTimeoutExpired; +} + +void prepareLapDetectionValues() { + upperSecondLevelRssiThreshold = maxDeepRssi - SECOND_LEVEL_RSSI_DETECTION_ADJUSTMENT; } void checkIfDroneLeftDeviceArea() { + if (didLeaveDeviceAreaThisLap) return; + if (rssi3 < lowerSecondLevelThreshold) { didLeaveDeviceAreaThisLap = true; + prepareLapDetectionValues(); } } @@ -88,6 +98,7 @@ bool checkIsLapDetected() { // --- the commented part in the condition below would make sure that the proximity is not tracked before threshold. is it needed? if (rssi > upperSecondLevelRssiThreshold - PROXIMITY_STEPS /*&& rssi > rssiThreshold*/) { isApproaching = true; + digitalHigh(ledPin); // debug only uint16_t diffWithThreshold = upperSecondLevelRssiThreshold - rssi; if (diffWithThreshold < currentProximityIndex) { @@ -143,13 +154,11 @@ bool checkIsLapDetected() { return false; } -void prepareLapDetectionValues() { - upperSecondLevelRssiThreshold = maxDeepRssi - SECOND_LEVEL_RSSI_DETECTION_ADJUSTMENT; -} - void resetFieldsAfterLapDetection() { + digitalLow(ledPin); isApproaching = false; - currentProximityIndex = 0xFFFF; + isLapDetectionTimeoutExpired = false; + currentProximityIndex = 0xFF; isFirstThresholdCrossed = false; didLeaveDeviceAreaThisLap = false; timeWhenFirstThresholdCrossed = 0; @@ -162,7 +171,8 @@ void resetFieldsAfterLapDetection() { void resetFieldsBeforeRaceStart() { isApproaching = false; - currentProximityIndex = 0xFFFF; + isLapDetectionTimeoutExpired = false; + currentProximityIndex = 0xFF; isFirstThresholdCrossed = false; maxDeepRssi = rssiThreshold - EDGE_RSSI_ADJUSTMENT; // must be below rssiThreshold minDeepRssi = rssiThreshold; diff --git a/Arduino/chorus_rf_laptimer/mainDetectionAlgorithm.h b/Arduino/chorus_rf_laptimer/mainDetectionAlgorithm.h index a96bf91..e6f9156 100644 --- a/Arduino/chorus_rf_laptimer/mainDetectionAlgorithm.h +++ b/Arduino/chorus_rf_laptimer/mainDetectionAlgorithm.h @@ -60,17 +60,12 @@ void runExperimentalLapDetectionAlgorithm() { } } else { if (checkIsMaxRssiDetectionTimeoutNotExpired()) { - findMaxRssi(); findMaxDeepRssi(); } findMinDeepRssi(); checkIfDroneLeftDeviceArea(); - if (didLeaveDeviceAreaThisLap) { - prepareLapDetectionValues(); - } - if (checkIsLapDetectionTimeoutExpired()) { if (didLeaveDeviceAreaThisLap) { if (checkIsLapDetected()) { diff --git a/Arduino/chorus_rf_laptimer/sounds.h b/Arduino/chorus_rf_laptimer/sounds.h index e585314..ecec1b6 100644 --- a/Arduino/chorus_rf_laptimer/sounds.h +++ b/Arduino/chorus_rf_laptimer/sounds.h @@ -71,6 +71,8 @@ uint16_t toneSeq_SetupThresholdMiddle [TONE_SEQ_SETUP_THRESHOLD_MIDDLE] = { 659, #define TONE_SEQ_SETUP_THRESHOLD_DONE 14 uint16_t toneSeq_SetupThresholdDone [TONE_SEQ_SETUP_THRESHOLD_DONE] = { 880, 150, 0, 30, 784, 150, 0, 30, 659, 150, 0, 30, 523, 500}; +#define TONE_SEQ_SINGLE_NOTE 2 +uint16_t toneSeq_SingleNote[TONE_SEQ_SINGLE_NOTE] = {0, 100}; // ---------------------------------------------------------------------------- void startPlayingTones() { @@ -137,4 +139,12 @@ void playThresholdSetupDoneTones() { curToneSeq = toneSeq_SetupThresholdDone; lastToneSeqIndex = TONE_SEQ_SETUP_THRESHOLD_DONE - 1; startPlayingTones(); -} \ No newline at end of file +} + +// ---------------------------------------------------------------------------- +void playNote(uint16_t noteFreq) { + curToneSeq = toneSeq_SingleNote; + lastToneSeqIndex = TONE_SEQ_SINGLE_NOTE - 1; + *curToneSeq = noteFreq; + startPlayingTones(); +}