Merge branch 'measure-speed' into display

board_motorctl/main/CMakeLists.txt

@@ -4,6 +4,7 @@ idf_component_register(
         "config.cpp"
         "fan.cpp"
 		"uart.cpp"
+		"speedsensor.cpp"
     INCLUDE_DIRS 
         "."
     )

board_motorctl/main/main.cpp

@@ -14,12 +14,20 @@ extern "C"
 
 #include "driver/ledc.h"
 
-//custom C files
+	//custom C files
 #include "wifi.h"
 }
 //custom C++ files
 #include "config.hpp"
 #include "uart.hpp"
+#include "speedsensor.hpp"
+
+
+//============================
+//======= TESTING MODE =======
+//============================
+//do not start the actual tasks for controlling the armchair
+#define TESTING_MODE
 
 //=========================
 //======= UART TEST =======
@@ -32,14 +40,18 @@ extern "C"
 //======= BRAKE TEST =======
 //==========================
 //only run brake-test (ignore uart input)
-#define BRAKE_TEST_ONLY
+//#define BRAKE_TEST_ONLY
 
+//====================-======
+//==== SPEED SENSOR TEST ====
+//======================-====
+//only run speed-sensor test
+#define SPEED_SENSOR_TEST
 
 //tag for logging
 static const char * TAG = "main";
 
 
-#ifndef UART_TEST_ONLY
 //====================================
 //========== motorctl task ===========
 //====================================
@@ -110,22 +122,25 @@ void setLoglevels(void){
 	esp_log_level_set("uart_common", ESP_LOG_INFO);
 	esp_log_level_set("uart", ESP_LOG_INFO);
 	//esp_log_level_set("current-sensors", ESP_LOG_INFO);
+	esp_log_level_set("speedSensor", ESP_LOG_WARN);
 }
-#endif
+
 
 
 //=================================
 //=========== app_main ============
 //=================================
 extern "C" void app_main(void) {
-#ifndef UART_TEST_ONLY
+
+	//---- define log levels ----
+	setLoglevels();
+
+#ifndef TESTING_MODE
 	//enable 5V volate regulator
 	gpio_pad_select_gpio(GPIO_NUM_17);                                                  
 	gpio_set_direction(GPIO_NUM_17, GPIO_MODE_OUTPUT);
 	gpio_set_level(GPIO_NUM_17, 1);                                                      
 
-	//---- define log levels ----
-	setLoglevels();
 
 	//----------------------------------------------
 	//--- create task for controlling the motors ---
@@ -152,20 +167,42 @@ extern "C" void app_main(void) {
 
 
 
+#ifndef BRAKE_TEST_ONLY
 	//-------------------------------------------
 	//--- create tasks for uart communication ---
 	//-------------------------------------------
-#ifndef BRAKE_TEST_ONLY
 	uart_init();
 	xTaskCreate(task_uartReceive, "task_uartReceive", 4096, NULL, 10, NULL);
 	xTaskCreate(task_uartSend, "task_uartSend", 4096, NULL, 10, NULL);
 #endif
 
 
-	//--- main loop ---
+
+#ifdef SPEED_SENSOR_TEST
+	speedSensor_config_t speedRight_config{
+		.gpioPin = GPIO_NUM_18,
+			.degreePerGroup = 72,
+			.tireCircumferenceMeter = 0.69,
+			.directionInverted = false,
+			.logName = "speedRight",
+	};
+	speedSensor speedRight (speedRight_config);
+#endif
+
+
+	//---------------------------
+	//-------- main loop --------
+	//---------------------------
 	//does nothing except for testing things
 	while(1){
-		vTaskDelay(500 / portTICK_PERIOD_MS);
+
+#ifdef SPEED_SENSOR_TEST
+		vTaskDelay(100 / portTICK_PERIOD_MS);
+		//speedRight.getRpm();
+		ESP_LOGI(TAG, "speedsensor-test: rpm=%fRPM, speed=%fkm/h dir=%d, pulseCount=%d, p1=%d, p2=%d, p3=%d lastEdgetime=%d", speedRight.getRpm(), speedRight.getKmph(), speedRight.direction, speedRight.pulseCounter, (int)speedRight.pulseDurations[0]/1000,  (int)speedRight.pulseDurations[1]/1000, (int)speedRight.pulseDurations[2]/1000,(int)speedRight.lastEdgeTime);
+#endif
+
+
 #ifdef BRAKE_TEST_ONLY
 		//test relays at standstill
 		ESP_LOGW("brake-test", "test relays via motorctl");
@@ -198,7 +235,6 @@ extern "C" void app_main(void) {
 		vTaskDelay(5000 / portTICK_PERIOD_MS);
 		//reset to idle
 		motorRight.setTarget(motorstate_t::IDLE, 0);
-	
 #endif
 
 

board_motorctl/main/speedsensor.cpp

@@ -0,0 +1,164 @@
+#include "speedsensor.hpp"
+#include "esp_timer.h"
+#include <ctime>
+
+//===== config =====
+#define TIMEOUT_NO_ROTATION 1000 //RPM set to 0 when no pulses within that time (ms)
+static const char* TAG = "speedSensor";
+
+
+
+uint32_t min(uint32_t a, uint32_t b){
+	if (a>b) return b;
+	else return a;
+}
+
+
+
+//=========================================
+//========== ISR onEncoderChange ==========
+//=========================================
+//handle gpio edge event
+//determines direction and rotational speed with a speedSensor object
+void IRAM_ATTR onEncoderChange(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
+
+		//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
+
+			//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));
+
+			//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));
+		}
+	}
+	//store current pin state for next edge detection
+	sensor->prevState = currentState;
+}
+
+
+
+
+//============================
+//======= constructor ========
+//============================
+speedSensor::speedSensor(speedSensor_config_t config_f){
+	//copy config
+	config = config_f;
+	//init gpio and ISR
+	init();
+}
+
+
+
+//==========================
+//========== init ==========
+//==========================
+ //initializes gpio pin and configures interrupt
+void speedSensor::init() {
+	//configure pin
+	gpio_pad_select_gpio(config.gpioPin);
+	gpio_set_direction(config.gpioPin, GPIO_MODE_INPUT);
+	gpio_set_pull_mode(config.gpioPin, GPIO_PULLUP_ONLY);
+	ESP_LOGW(TAG, "%s, configured gpio-pin %d", config.logName, (int)config.gpioPin);
+
+	//configure interrupt
+	gpio_set_intr_type(config.gpioPin, GPIO_INTR_ANYEDGE);
+	gpio_install_isr_service(0);
+	gpio_isr_handler_add(config.gpioPin, onEncoderChange, this);
+	ESP_LOGW(TAG, "%s, configured interrupt", config.logName);
+}
+
+
+
+
+//==========================
+//========= getRpm =========
+//==========================
+//get rotational speed in revolutions per minute
+float speedSensor::getRpm(){
+	uint32_t timeElapsed = esp_timer_get_time() - lastEdgeTime;
+	//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",
+				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);
+
+	//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);
+	return currentSpeed;
+}
+
+
+//==========================
+//========= getMps =========
+//==========================
+//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);
+	return currentSpeed;
+}

board_motorctl/main/speedsensor.hpp

@@ -0,0 +1,53 @@
+#pragma once
+extern "C" {
+#include "esp_log.h"
+#include "hal/gpio_types.h"
+#include "driver/gpio.h"
+#include "esp_timer.h"
+}
+
+//Encoder disk requirements:
+//encoder disk has to have gaps in 3 differnt intervals (short, medium, long)
+//that pattern can be repeated multiple times, see config option
+typedef struct {
+    gpio_num_t gpioPin;
+	float degreePerGroup;	//360 / [count of short,medium,long groups on encoder disk]
+	float tireCircumferenceMeter;
+	//positive direction is pulse order "short, medium, long"
+	bool directionInverted;
+	char* logName;
+} speedSensor_config_t;
+
+
+class speedSensor {
+	//TODO add count of revolutions/pulses if needed? (get(), reset() etc)
+public:
+	//constructor
+    speedSensor(speedSensor_config_t config);
+ //initializes gpio pin and configures interrupt
+    void init();
+	
+	//negative values = reverse direction
+	//positive values = forward direction
+	float getKmph(); //kilometers per hour
+	float getMps(); //meters per second
+	float getRpm();  //rotations per minute
+
+	//1=forward, -1=reverse
+    int direction;
+
+	//variables for handling the encoder
+	speedSensor_config_t config;
+    int prevState = 0;
+	uint64_t pulseDurations[3] = {};
+	uint64_t lastEdgeTime = 0;
+	uint8_t pulseCounter = 0;
+	int debugCount = 0;
+	double currentRpm = 0;
+
+private:
+
+};
+
+
+