From db4e6b56a52fa53d16366602698471f37a542a8a Mon Sep 17 00:00:00 2001 From: jonny_jr9 Date: Mon, 26 Feb 2024 11:12:35 +0100 Subject: [PATCH 1/2] Rework direction-detection, ignore bad pulses, optimize - trigger on rising edge only - ignore too short pulses - possible noise (config option) - ignore invalid pulse orders - debug log count of ignored sequences --- board_single/main/config.cpp | 6 +- common/speedsensor.cpp | 132 +++++++++++++++++++++-------------- common/speedsensor.hpp | 12 ++-- 3 files changed, 90 insertions(+), 60 deletions(-) diff --git a/board_single/main/config.cpp b/board_single/main/config.cpp index 82cde90..cd4ba20 100644 --- a/board_single/main/config.cpp +++ b/board_single/main/config.cpp @@ -176,7 +176,8 @@ fan_config_t configFans = { speedSensor_config_t speedLeft_config{ .gpioPin = GPIO_NUM_5, .degreePerGroup = 360 / 5, - .tireCircumferenceMeter = 210.0 * 3.141 / 1000.0, + .minPulseDurationUs = 10000, //smallest possible pulse duration (< time from start small-pulse to start long-pulse at full speed). Set to 0 to disable this noise detection + .tireCircumferenceMeter = 0.81, .directionInverted = false, .logName = "speedLeft", }; @@ -184,7 +185,8 @@ speedSensor_config_t speedLeft_config{ speedSensor_config_t speedRight_config{ .gpioPin = GPIO_NUM_14, .degreePerGroup = 360 / 12, - .tireCircumferenceMeter = 210.0 * 3.141 / 1000.0, + .minPulseDurationUs = 10000, //smallest possible pulse duration (< time from start small-pulse to start long-pulse at full speed). Set to 0 to disable this noise detection + .tireCircumferenceMeter = 0.81, .directionInverted = true, .logName = "speedRight", }; diff --git a/common/speedsensor.cpp b/common/speedsensor.cpp index 643c128..d935138 100644 --- a/common/speedsensor.cpp +++ b/common/speedsensor.cpp @@ -19,63 +19,85 @@ uint32_t min(uint32_t a, uint32_t b){ //========================================= -//========== ISR onEncoderChange ========== +//========== ISR onEncoderRising ========== //========================================= -//handle gpio edge event +//handle gpio rising edge event //determines direction and rotational speed with a speedSensor object -void IRAM_ATTR onEncoderChange(void* arg) { - speedSensor* sensor = (speedSensor*)arg; +void IRAM_ATTR onEncoderRising(void *arg) +{ + speedSensor *sensor = (speedSensor *)arg; int currentState = gpio_get_level(sensor->config.gpioPin); - //detect rising edge LOW->HIGH (reached end of gap in encoder disk) - if (currentState == 1 && sensor->prevState == 0) { - //time since last edge in us - uint32_t currentTime = esp_timer_get_time(); - uint32_t timeElapsed = currentTime - sensor->lastEdgeTime; - sensor->lastEdgeTime = currentTime; //update last edge time + // time since last edge in us + uint32_t currentTime = esp_timer_get_time(); + uint32_t timeElapsed = currentTime - sensor->lastEdgeTime; + sensor->lastEdgeTime = currentTime; // update last edge time - //store duration of last pulse - sensor->pulseDurations[sensor->pulseCounter] = timeElapsed; - sensor->pulseCounter++; + // store duration of last pulse + sensor->pulseDurations[sensor->pulseCounter] = timeElapsed; + sensor->pulseCounter++; - //check if 3rd pulse has occoured - if (sensor->pulseCounter >= 3) { - sensor->pulseCounter = 0; //reset counter + // check if 3rd pulse has occoured (one sequence recorded) + if (sensor->pulseCounter >= 3) + { + sensor->pulseCounter = 0; // reset count - //simplify variable names - uint32_t pulse1 = sensor->pulseDurations[0]; - uint32_t pulse2 = sensor->pulseDurations[1]; - uint32_t pulse3 = sensor->pulseDurations[2]; + // simplify variable names + uint32_t pulse1 = sensor->pulseDurations[0]; + uint32_t pulse2 = sensor->pulseDurations[1]; + uint32_t pulse3 = sensor->pulseDurations[2]; - //find shortest pulse - uint32_t shortestPulse = min(pulse1, min(pulse2, pulse3)); + // find shortest pulse + sensor->shortestPulse = min(pulse1, min(pulse2, pulse3)); - //Determine direction based on pulse order - int directionNew = 0; - if (shortestPulse == pulse1) { //short-medium-long... - directionNew = 1; //fwd - } else if (shortestPulse == pulse3) { //long-medium-short... - directionNew = -1; //rev - } else if (shortestPulse == pulse2) { - if (pulse1 < pulse3){ //medium short long-medium-short long... - directionNew = -1; //rev - } else { //long short-medium-long short-medium-long... - directionNew = 1; //fwd - } - } - - //save and invert direction if necessay - //TODO mutex? - if (sensor->config.directionInverted) sensor->direction = -directionNew; - else sensor->direction = directionNew; - - //calculate rotational speed - uint64_t pulseSum = pulse1 + pulse2 + pulse3; - sensor->currentRpm = directionNew * (sensor->config.degreePerGroup / 360.0 * 60.0 / ((double)pulseSum / 1000000.0)); + // ignore this pulse sequence if one pulse is too short (possible noise) + if (sensor->shortestPulse < sensor->config.minPulseDurationUs) + { + sensor->debug_countIgnoredSequencesTooShort++; + return; } + + //-- Determine direction based on pulse order --- + int directionNew = 0; + if (sensor->shortestPulse == pulse1) // short... + { + if (pulse2 < pulse3) // short-medium-long + directionNew = 1; + else // short-long-medium (invaild) + { + sensor->debug_countIgnoredSequencesInvalidOrder++; + return; + }; + } + else if (sensor->shortestPulse == pulse3) //...short + { + if (pulse1 > pulse2) // long-medium-short + directionNew = -1; + else // medium-long-short (invaild) + { + sensor->debug_countIgnoredSequencesInvalidOrder++; + return; + }; + } + else if (sensor->shortestPulse == pulse2) //...short... + { + // medium-short-long (invalid) + // long-short-medium (invalid) + sensor->debug_countIgnoredSequencesInvalidOrder++; + return; + } + + // save and invert direction if necessay + // TODO mutex? + if (sensor->config.directionInverted) + sensor->direction = -directionNew; + else + sensor->direction = directionNew; + + // calculate rotational speed + uint64_t pulseSum = pulse1 + pulse2 + pulse3; + sensor->currentRpm = directionNew * (sensor->config.degreePerGroup / 360.0 * 60.0 / ((double)pulseSum / 1000000.0)); } - //store current pin state for next edge detection - sensor->prevState = currentState; } @@ -104,13 +126,13 @@ void speedSensor::init() { gpio_set_pull_mode(config.gpioPin, GPIO_PULLUP_ONLY); //configure interrupt - gpio_set_intr_type(config.gpioPin, GPIO_INTR_ANYEDGE); + gpio_set_intr_type(config.gpioPin, GPIO_INTR_POSEDGE); if (!isrIsInitialized) { gpio_install_isr_service(0); isrIsInitialized = true; ESP_LOGW(TAG, "Initialized ISR service"); } - gpio_isr_handler_add(config.gpioPin, onEncoderChange, this); + gpio_isr_handler_add(config.gpioPin, onEncoderRising, this); ESP_LOGW(TAG, "[%s], configured gpio-pin %d and interrupt routine", config.logName, (int)config.gpioPin); } @@ -131,8 +153,8 @@ float speedSensor::getRpm(){ currentRpm = 0; } //debug output (also log variables when this function is called) - ESP_LOGI(TAG, "%s - getRpm: returning stored rpm=%.3f", config.logName, currentRpm); - ESP_LOGV(TAG, "%s - rpm=%f, dir=%d, pulseCount=%d, p1=%d, p2=%d, p3=%d lastEdgetime=%d", + ESP_LOGI(TAG, "[%s] getRpm: returning stored rpm=%.3f", config.logName, currentRpm); + ESP_LOGV(TAG, "[%s] rpm=%f, dir=%d, pulseCount=%d, p1=%d, p2=%d, p3=%d, shortest=%d, tooShortCount=%d, invalidOrderCount=%d", config.logName, currentRpm, direction, @@ -140,7 +162,9 @@ float speedSensor::getRpm(){ (int)pulseDurations[0]/1000, (int)pulseDurations[1]/1000, (int)pulseDurations[2]/1000, - (int)lastEdgeTime); + shortestPulse, + debug_countIgnoredSequencesTooShort, + debug_countIgnoredSequencesInvalidOrder); //return currently stored rpm return currentRpm; @@ -148,9 +172,9 @@ float speedSensor::getRpm(){ -//========================== +//=========================== //========= getKmph ========= -//========================== +//=========================== //get speed in kilometers per hour float speedSensor::getKmph(){ float currentSpeed = getRpm() * config.tireCircumferenceMeter * 60/1000; @@ -164,7 +188,7 @@ float speedSensor::getKmph(){ //========================== //get speed in meters per second float speedSensor::getMps(){ - float currentSpeed = getRpm() * config.tireCircumferenceMeter; + float currentSpeed = getRpm() * config.tireCircumferenceMeter / 60; ESP_LOGI(TAG, "%s - getMps: returning speed=%.3fm/s", config.logName, currentSpeed); return currentSpeed; } diff --git a/common/speedsensor.hpp b/common/speedsensor.hpp index a07359d..c06df00 100644 --- a/common/speedsensor.hpp +++ b/common/speedsensor.hpp @@ -12,8 +12,9 @@ extern "C" { typedef struct { gpio_num_t gpioPin; float degreePerGroup; //360 / [count of short,medium,long groups on encoder disk] + uint32_t minPulseDurationUs; //smallest possible pulse duration (time from start small-pulse to start long-pulse at full speed). Set to 0 to disable this noise detection float tireCircumferenceMeter; - //positive direction is pulse order "short, medium, long" + //default positive direction is pulse order "short, medium, long" bool directionInverted; char* logName; } speedSensor_config_t; @@ -37,11 +38,14 @@ public: int direction; //variables for handling the encoder (public because ISR needs access) speedSensor_config_t config; - int prevState = 0; - uint64_t pulseDurations[3] = {}; - uint64_t lastEdgeTime = 0; + uint32_t pulseDurations[3] = {}; + uint32_t shortestPulse = 0; + uint32_t shortestPulsePrev = 0; + uint32_t lastEdgeTime = 0; uint8_t pulseCounter = 0; int debugCount = 0; + uint32_t debug_countIgnoredSequencesTooShort = 0; + uint32_t debug_countIgnoredSequencesInvalidOrder = 0; double currentRpm = 0; private: From fbca35e828384e1122f453e36d0c3a34a788ff01 Mon Sep 17 00:00:00 2001 From: jonny_l480 Date: Mon, 26 Feb 2024 22:45:58 +0100 Subject: [PATCH 2/2] Fix direction-detection, Fix inversion - reliable now Tested and debugged speed measurement on actual hardware. Speed measurement is very stable now (reliable direction) - fix direction inverted no effect - fix direction detection - remove "invalid directions" since every combination is valid --- common/speedsensor.cpp | 75 +++++++++++++++++++----------------------- common/speedsensor.hpp | 4 +-- 2 files changed, 35 insertions(+), 44 deletions(-) diff --git a/common/speedsensor.cpp b/common/speedsensor.cpp index d935138..3d95a66 100644 --- a/common/speedsensor.cpp +++ b/common/speedsensor.cpp @@ -47,6 +47,11 @@ void IRAM_ATTR onEncoderRising(void *arg) uint32_t pulse2 = sensor->pulseDurations[1]; uint32_t pulse3 = sensor->pulseDurations[2]; + // save all recored pulses of this sequence (for logging only) + sensor->pulse1 = pulse1; + sensor->pulse2 = pulse2; + sensor->pulse3 = pulse3; + // find shortest pulse sensor->shortestPulse = min(pulse1, min(pulse2, pulse3)); @@ -57,46 +62,37 @@ void IRAM_ATTR onEncoderRising(void *arg) return; } - //-- Determine direction based on pulse order --- - int directionNew = 0; + //--- Determine direction based on pulse order --- + int direction = 0; if (sensor->shortestPulse == pulse1) // short... { - if (pulse2 < pulse3) // short-medium-long - directionNew = 1; - else // short-long-medium (invaild) - { - sensor->debug_countIgnoredSequencesInvalidOrder++; - return; - }; + if (pulse2 < pulse3) // short-medium-long --> + direction = 1; + else // short-long-medium <-- + direction = -1; } else if (sensor->shortestPulse == pulse3) //...short { - if (pulse1 > pulse2) // long-medium-short - directionNew = -1; - else // medium-long-short (invaild) - { - sensor->debug_countIgnoredSequencesInvalidOrder++; - return; - }; + if (pulse1 > pulse2) // long-medium-short <-- + direction = -1; + else // medium-long-short --> + direction = 1; } else if (sensor->shortestPulse == pulse2) //...short... { - // medium-short-long (invalid) - // long-short-medium (invalid) - sensor->debug_countIgnoredSequencesInvalidOrder++; - return; + if (pulse1 < pulse3) // medium-short-long + direction = -1; + else // long-short-medium + direction = 1; } // save and invert direction if necessay - // TODO mutex? if (sensor->config.directionInverted) - sensor->direction = -directionNew; - else - sensor->direction = directionNew; + direction = -direction; // calculate rotational speed uint64_t pulseSum = pulse1 + pulse2 + pulse3; - sensor->currentRpm = directionNew * (sensor->config.degreePerGroup / 360.0 * 60.0 / ((double)pulseSum / 1000000.0)); + sensor->currentRpm = direction * (sensor->config.degreePerGroup / 360.0 * 60.0 / ((double)pulseSum / 1000000.0)); } } @@ -148,24 +144,21 @@ float speedSensor::getRpm(){ //timeout (standstill) //TODO variable timeout considering config.degreePerGroup if ((currentRpm != 0) && (esp_timer_get_time() - lastEdgeTime) > TIMEOUT_NO_ROTATION*1000){ - ESP_LOGW(TAG, "%s - timeout: no pulse within %dms... last pulse was %dms ago => set RPM to 0", + ESP_LOGI(TAG, "%s - timeout: no pulse within %dms... last pulse was %dms ago => set RPM to 0", config.logName, TIMEOUT_NO_ROTATION, timeElapsed/1000); currentRpm = 0; } //debug output (also log variables when this function is called) - ESP_LOGI(TAG, "[%s] getRpm: returning stored rpm=%.3f", config.logName, currentRpm); - ESP_LOGV(TAG, "[%s] rpm=%f, dir=%d, pulseCount=%d, p1=%d, p2=%d, p3=%d, shortest=%d, tooShortCount=%d, invalidOrderCount=%d", - config.logName, - currentRpm, - direction, - pulseCounter, - (int)pulseDurations[0]/1000, - (int)pulseDurations[1]/1000, - (int)pulseDurations[2]/1000, - shortestPulse, - debug_countIgnoredSequencesTooShort, - debug_countIgnoredSequencesInvalidOrder); - + ESP_LOGD(TAG, "[%s] getRpm: returning stored rpm=%.3f", config.logName, currentRpm); + ESP_LOGV(TAG, "[%s] rpm=%f, pulseCount=%d, p1=%d, p2=%d, p3=%d, shortest=%d, totalTooShortCount=%d", + config.logName, + currentRpm, + pulseCounter, + pulse1 / 1000, + pulse2 / 1000, + pulse3 / 1000, + shortestPulse / 1000, + debug_countIgnoredSequencesTooShort); //return currently stored rpm return currentRpm; } @@ -178,7 +171,7 @@ float speedSensor::getRpm(){ //get speed in kilometers per hour float speedSensor::getKmph(){ float currentSpeed = getRpm() * config.tireCircumferenceMeter * 60/1000; - ESP_LOGI(TAG, "%s - getKmph: returning speed=%.3fkm/h", config.logName, currentSpeed); + ESP_LOGD(TAG, "%s - getKmph: returning speed=%.3fkm/h", config.logName, currentSpeed); return currentSpeed; } @@ -189,6 +182,6 @@ float speedSensor::getKmph(){ //get speed in meters per second float speedSensor::getMps(){ float currentSpeed = getRpm() * config.tireCircumferenceMeter / 60; - ESP_LOGI(TAG, "%s - getMps: returning speed=%.3fm/s", config.logName, currentSpeed); + ESP_LOGD(TAG, "%s - getMps: returning speed=%.3fm/s", config.logName, currentSpeed); return currentSpeed; } diff --git a/common/speedsensor.hpp b/common/speedsensor.hpp index c06df00..f559ec3 100644 --- a/common/speedsensor.hpp +++ b/common/speedsensor.hpp @@ -34,18 +34,16 @@ public: float getMps(); //meters per second float getRpm(); //rotations per minute - //1=forward, -1=reverse - int direction; //variables for handling the encoder (public because ISR needs access) speedSensor_config_t config; uint32_t pulseDurations[3] = {}; + uint32_t pulse1, pulse2, pulse3; uint32_t shortestPulse = 0; uint32_t shortestPulsePrev = 0; uint32_t lastEdgeTime = 0; uint8_t pulseCounter = 0; int debugCount = 0; uint32_t debug_countIgnoredSequencesTooShort = 0; - uint32_t debug_countIgnoredSequencesInvalidOrder = 0; double currentRpm = 0; private: