j/armchair_fw
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rework direction-detection, ignore bad pulses, optimize

Browse Source 
- trigger on rising edge only
- ignore too short pulses - possible noise (config option)
- ignore invalid pulse orders
- debug log count of ignored sequences
This commit is contained in:
jonny_jr9 2024-02-26 11:12:35 +01:00
parent de4c75a9b2
commit db4e6b56a5
3 changed files with 90 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
board_single/main/config.cpp
View File

@ -176,7 +176,8 @@ fan_config_t configFans = {
speedSensor_config_t speedLeft_config{ speedSensor_config_t speedLeft_config{
.gpioPin = GPIO_NUM_5, .gpioPin = GPIO_NUM_5,
.degreePerGroup = 360 / 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, .directionInverted = false,
.logName = "speedLeft", .logName = "speedLeft",
}; };
@ -184,7 +185,8 @@ speedSensor_config_t speedLeft_config{
speedSensor_config_t speedRight_config{ speedSensor_config_t speedRight_config{
.gpioPin = GPIO_NUM_14, .gpioPin = GPIO_NUM_14,
.degreePerGroup = 360 / 12, .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, .directionInverted = true,
.logName = "speedRight", .logName = "speedRight",
}; };

132
common/speedsensor.cpp
View File

@ -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 //determines direction and rotational speed with a speedSensor object
void IRAM_ATTR onEncoderChange(void* arg) { void IRAM_ATTR onEncoderRising(void *arg)
speedSensor* sensor = (speedSensor*)arg; {
speedSensor *sensor = (speedSensor *)arg;
int currentState = gpio_get_level(sensor->config.gpioPin); int currentState = gpio_get_level(sensor->config.gpioPin);
//detect rising edge LOW->HIGH (reached end of gap in encoder disk) // time since last edge in us
if (currentState == 1 && sensor->prevState == 0) { uint32_t currentTime = esp_timer_get_time();
//time since last edge in us uint32_t timeElapsed = currentTime - sensor->lastEdgeTime;
uint32_t currentTime = esp_timer_get_time(); sensor->lastEdgeTime = currentTime; // update last edge time
uint32_t timeElapsed = currentTime - sensor->lastEdgeTime;
sensor->lastEdgeTime = currentTime; //update last edge time
//store duration of last pulse // store duration of last pulse
sensor->pulseDurations[sensor->pulseCounter] = timeElapsed; sensor->pulseDurations[sensor->pulseCounter] = timeElapsed;
sensor->pulseCounter++; sensor->pulseCounter++;
//check if 3rd pulse has occoured // check if 3rd pulse has occoured (one sequence recorded)
if (sensor->pulseCounter >= 3) { if (sensor->pulseCounter >= 3)
sensor->pulseCounter = 0; //reset counter {
sensor->pulseCounter = 0; // reset count
//simplify variable names // simplify variable names
uint32_t pulse1 = sensor->pulseDurations[0]; uint32_t pulse1 = sensor->pulseDurations[0];
uint32_t pulse2 = sensor->pulseDurations[1]; uint32_t pulse2 = sensor->pulseDurations[1];
uint32_t pulse3 = sensor->pulseDurations[2]; uint32_t pulse3 = sensor->pulseDurations[2];
//find shortest pulse // find shortest pulse
uint32_t shortestPulse = min(pulse1, min(pulse2, pulse3)); sensor->shortestPulse = min(pulse1, min(pulse2, pulse3));
//Determine direction based on pulse order // ignore this pulse sequence if one pulse is too short (possible noise)
int directionNew = 0; if (sensor->shortestPulse < sensor->config.minPulseDurationUs)
if (shortestPulse == pulse1) { //short-medium-long... {
directionNew = 1; //fwd sensor->debug_countIgnoredSequencesTooShort++;
} else if (shortestPulse == pulse3) { //long-medium-short... return;
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));
} }
//-- 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); gpio_set_pull_mode(config.gpioPin, GPIO_PULLUP_ONLY);
//configure interrupt //configure interrupt
gpio_set_intr_type(config.gpioPin, GPIO_INTR_ANYEDGE); gpio_set_intr_type(config.gpioPin, GPIO_INTR_POSEDGE);
if (!isrIsInitialized) { if (!isrIsInitialized) {
gpio_install_isr_service(0); gpio_install_isr_service(0);
isrIsInitialized = true; isrIsInitialized = true;
ESP_LOGW(TAG, "Initialized ISR service"); 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); 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; currentRpm = 0;
} }
//debug output (also log variables when this function is called) //debug output (also log variables when this function is called)
ESP_LOGI(TAG, "%s - getRpm: returning stored rpm=%.3f", config.logName, currentRpm); 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_LOGV(TAG, "[%s] rpm=%f, dir=%d, pulseCount=%d, p1=%d, p2=%d, p3=%d, shortest=%d, tooShortCount=%d, invalidOrderCount=%d",
config.logName, config.logName,
currentRpm, currentRpm,
direction, direction,
@ -140,7 +162,9 @@ float speedSensor::getRpm(){
(int)pulseDurations[0]/1000, (int)pulseDurations[0]/1000,
(int)pulseDurations[1]/1000, (int)pulseDurations[1]/1000,
(int)pulseDurations[2]/1000, (int)pulseDurations[2]/1000,
(int)lastEdgeTime); shortestPulse,
debug_countIgnoredSequencesTooShort,
debug_countIgnoredSequencesInvalidOrder);
//return currently stored rpm //return currently stored rpm
return currentRpm; return currentRpm;
@ -148,9 +172,9 @@ float speedSensor::getRpm(){
//========================== //===========================
//========= getKmph ========= //========= getKmph =========
//========================== //===========================
//get speed in kilometers per hour //get speed in kilometers per hour
float speedSensor::getKmph(){ float speedSensor::getKmph(){
float currentSpeed = getRpm() * config.tireCircumferenceMeter * 60/1000; float currentSpeed = getRpm() * config.tireCircumferenceMeter * 60/1000;
@ -164,7 +188,7 @@ float speedSensor::getKmph(){
//========================== //==========================
//get speed in meters per second //get speed in meters per second
float speedSensor::getMps(){ 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); ESP_LOGI(TAG, "%s - getMps: returning speed=%.3fm/s", config.logName, currentSpeed);
return currentSpeed; return currentSpeed;
} }

12
common/speedsensor.hpp
View File

@ -12,8 +12,9 @@ extern "C" {
typedef struct { typedef struct {
gpio_num_t gpioPin; gpio_num_t gpioPin;
float degreePerGroup; //360 / [count of short,medium,long groups on encoder disk] 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; float tireCircumferenceMeter;
//positive direction is pulse order "short, medium, long" //default positive direction is pulse order "short, medium, long"
bool directionInverted; bool directionInverted;
char* logName; char* logName;
} speedSensor_config_t; } speedSensor_config_t;
@ -37,11 +38,14 @@ public:
int direction; int direction;
//variables for handling the encoder (public because ISR needs access) //variables for handling the encoder (public because ISR needs access)
speedSensor_config_t config; speedSensor_config_t config;
int prevState = 0; uint32_t pulseDurations[3] = {};
uint64_t pulseDurations[3] = {}; uint32_t shortestPulse = 0;
uint64_t lastEdgeTime = 0; uint32_t shortestPulsePrev = 0;
uint32_t lastEdgeTime = 0;
uint8_t pulseCounter = 0; uint8_t pulseCounter = 0;
int debugCount = 0; int debugCount = 0;
uint32_t debug_countIgnoredSequencesTooShort = 0;
uint32_t debug_countIgnoredSequencesInvalidOrder = 0;
double currentRpm = 0; double currentRpm = 0;
private: private: