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{
.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",
};

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
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));
// save all recored pulses of this sequence (for logging only)
sensor->pulse1 = pulse1;
sensor->pulse2 = pulse2;
sensor->pulse3 = 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
}
}
// find shortest pulse
sensor->shortestPulse = min(pulse1, min(pulse2, pulse3));
//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 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);
//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);
}
@ -126,35 +144,34 @@ 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 lastEdgetime=%d",
config.logName,
currentRpm,
direction,
pulseCounter,
(int)pulseDurations[0]/1000,
(int)pulseDurations[1]/1000,
(int)pulseDurations[2]/1000,
(int)lastEdgeTime);
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;
}
//==========================
//===========================
//========= getKmph =========
//==========================
//===========================
//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;
}
@ -164,7 +181,7 @@ float speedSensor::getKmph(){
//==========================
//get speed in meters per second
float speedSensor::getMps(){
float currentSpeed = getRpm() * config.tireCircumferenceMeter;
ESP_LOGI(TAG, "%s - getMps: returning speed=%.3fm/s", config.logName, currentSpeed);
float currentSpeed = getRpm() * config.tireCircumferenceMeter / 60;
ESP_LOGD(TAG, "%s - getMps: returning speed=%.3fm/s", config.logName, currentSpeed);
return currentSpeed;
}

14
common/speedsensor.hpp
View File

@ -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;
@ -33,15 +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;
int prevState = 0;
uint64_t pulseDurations[3] = {};
uint64_t lastEdgeTime = 0;
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;
double currentRpm = 0;
private: