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

Merge branch 'measure-speed' into dev - speedsensors stable

Browse Source
This commit is contained in:
jonny_l480 2024-02-26 22:49:26 +01:00
commit 5b198824f2
3 changed files with 93 additions and 72 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",
}; };

145
common/speedsensor.cpp
View File

@ -19,63 +19,81 @@ 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 // save all recored pulses of this sequence (for logging only)
uint32_t shortestPulse = min(pulse1, min(pulse2, pulse3)); sensor->pulse1 = pulse1;
sensor->pulse2 = pulse2;
sensor->pulse3 = pulse3;
//Determine direction based on pulse order // find shortest pulse
int directionNew = 0; sensor->shortestPulse = min(pulse1, min(pulse2, pulse3));
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 // ignore this pulse sequence if one pulse is too short (possible noise)
//TODO mutex? if (sensor->shortestPulse < sensor->config.minPulseDurationUs)
if (sensor->config.directionInverted) sensor->direction = -directionNew; {
else sensor->direction = directionNew; sensor->debug_countIgnoredSequencesTooShort++;
return;
//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 direction = 0;
if (sensor->shortestPulse == pulse1) // short...
{
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 <--
direction = -1;
else // medium-long-short -->
direction = 1;
}
else if (sensor->shortestPulse == pulse2) //...short...
{
if (pulse1 < pulse3) // medium-short-long
direction = -1;
else // long-short-medium
direction = 1;
}
// save and invert direction if necessay
if (sensor->config.directionInverted)
direction = -direction;
// calculate rotational speed
uint64_t pulseSum = pulse1 + pulse2 + pulse3;
sensor->currentRpm = direction * (sensor->config.degreePerGroup / 360.0 * 60.0 / ((double)pulseSum / 1000000.0));
} }
//store current pin state for next edge detection
sensor->prevState = currentState;
} }
@ -104,13 +122,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);
} }
@ -126,35 +144,34 @@ float speedSensor::getRpm(){
//timeout (standstill) //timeout (standstill)
//TODO variable timeout considering config.degreePerGroup //TODO variable timeout considering config.degreePerGroup
if ((currentRpm != 0) && (esp_timer_get_time() - lastEdgeTime) > TIMEOUT_NO_ROTATION*1000){ 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); config.logName, TIMEOUT_NO_ROTATION, timeElapsed/1000);
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_LOGD(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, pulseCount=%d, p1=%d, p2=%d, p3=%d, shortest=%d, totalTooShortCount=%d",
config.logName, config.logName,
currentRpm, currentRpm,
direction, pulseCounter,
pulseCounter, pulse1 / 1000,
(int)pulseDurations[0]/1000, pulse2 / 1000,
(int)pulseDurations[1]/1000, pulse3 / 1000,
(int)pulseDurations[2]/1000, shortestPulse / 1000,
(int)lastEdgeTime); debug_countIgnoredSequencesTooShort);
//return currently stored rpm //return currently stored rpm
return currentRpm; return currentRpm;
} }
//========================== //===========================
//========= 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;
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; return currentSpeed;
} }
@ -164,7 +181,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_LOGD(TAG, "%s - getMps: returning speed=%.3fm/s", config.logName, currentSpeed);
return currentSpeed; return currentSpeed;
} }

14
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;
@ -33,15 +34,16 @@ public:
float getMps(); //meters per second float getMps(); //meters per second
float getRpm(); //rotations per minute float getRpm(); //rotations per minute
//1=forward, -1=reverse
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 pulse1, pulse2, pulse3;
uint64_t lastEdgeTime = 0; uint32_t shortestPulse = 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;
double currentRpm = 0; double currentRpm = 0;
private: private: