Pedal assist support (PAS)

appconf/appconf_default.h

@@ -392,6 +392,44 @@
 #define APPCONF_BALANCE_KD_PT1_FREQUENCY			0
 #endif
 
+// PAS app
+#ifndef APPCONF_PAS_CTRL_TYPE
+#define APPCONF_PAS_CTRL_TYPE				PAS_CTRL_TYPE_NONE
+#endif
+#ifndef APPCONF_PAS_SENSOR_TYPE
+#define APPCONF_PAS_SENSOR_TYPE				PAS_SENSOR_TYPE_QUADRATURE
+#endif
+#ifndef APPCONF_PAS_PEDAL_RPM_START
+#define APPCONF_PAS_PEDAL_RPM_START			10.0
+#endif
+#ifndef APPCONF_PAS_PEDAL_RPM_END
+#define APPCONF_PAS_PEDAL_RPM_END			180.0
+#endif
+#ifndef APPCONF_PAS_INVERT_PEDAL_DIRECTION
+#define APPCONF_PAS_INVERT_PEDAL_DIRECTION	false
+#endif
+#ifndef APPCONF_PAS_MAGNETS
+#define APPCONF_PAS_MAGNETS					24
+#endif
+#ifndef APPCONF_PAS_USE_FILTER
+#define APPCONF_PAS_USE_FILTER				true
+#endif
+#ifndef APPCONF_PAS_SAFE_START
+#define APPCONF_PAS_SAFE_START				true
+#endif
+#ifndef APPCONF_PAS_CURRENT_SCALING
+#define APPCONF_PAS_CURRENT_SCALING			0.1
+#endif
+#ifndef APPCONF_PAS_RAMP_TIME_POS
+#define APPCONF_PAS_RAMP_TIME_POS			0.6
+#endif
+#ifndef APPCONF_PAS_RAMP_TIME_NEG
+#define APPCONF_PAS_RAMP_TIME_NEG			0.3
+#endif
+#ifndef APPCONF_PAS_UPDATE_RATE_HZ
+#define APPCONF_PAS_UPDATE_RATE_HZ			500
+#endif
+
 // IMU
 #ifndef APPCONF_IMU_TYPE
 #define APPCONF_IMU_TYPE					IMU_TYPE_INTERNAL

applications/app.c

@@ -54,6 +54,7 @@ void app_set_configuration(app_configuration *conf) {
 	app_uartcomm_stop();
 	app_nunchuk_stop();
 	app_balance_stop();
+	app_pas_stop();
 
 	if (!conf_general_permanent_nrf_found) {
 		nrf_driver_stop();
@@ -110,6 +111,15 @@ void app_set_configuration(app_configuration *conf) {
 		}
 		break;
 
+	case APP_PAS:
+		app_pas_start(true);
+		break;
+
+	case APP_ADC_PAS:
+		app_adc_start(true);
+		app_pas_start(false);
+		break;
+
 	case APP_NRF:
 		if (!conf_general_permanent_nrf_found) {
 			nrf_driver_init();
@@ -130,6 +140,7 @@ void app_set_configuration(app_configuration *conf) {
 
 	app_ppm_configure(&appconf.app_ppm_conf);
 	app_adc_configure(&appconf.app_adc_conf);
+	app_pas_configure(&appconf.app_pas_conf);
 	app_uartcomm_configure(appconf.app_uart_baudrate, appconf.permanent_uart_enabled);
 	app_nunchuk_configure(&appconf.app_chuk_conf);
 

applications/app.h

@@ -70,6 +70,12 @@ uint16_t app_balance_get_switch_state(void);
 float app_balance_get_adc1(void);
 float app_balance_get_adc2(void);
 
+void app_pas_start(bool is_primary_output);
+void app_pas_stop(void);
+bool app_pas_is_running(void);
+void app_pas_configure(pas_config *conf);
+float app_pas_get_current_target_rel(void);
+
 // Custom apps
 void app_custom_start(void);
 void app_custom_stop(void);

applications/app_adc.c

@@ -337,6 +337,10 @@ static THD_FUNCTION(adc_thread, arg) {
 		case ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_ADC:
 			current_mode = true;
 			if (pwr >= 0.0) {
+				// if pedal assist (PAS) thread is running, use the highest current command
+				if (app_pas_is_running()) {
+					pwr = utils_max_abs(pwr, app_pas_get_current_target_rel());
+				}
 				current_rel = pwr;
 			} else {
 				current_rel = fabsf(pwr);

applications/app_pas.c

@@ -0,0 +1,243 @@
+/*
+	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2020 Marcos Chaparro	mchaparro@powerdesigns.ca
+
+	This file is part of the VESC firmware.
+
+	The VESC firmware is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    The VESC firmware is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "app.h"
+
+#include "ch.h"
+#include "hal.h"
+#include "stm32f4xx_conf.h"
+#include "mc_interface.h"
+#include "timeout.h"
+#include "utils.h"
+#include "comm_can.h"
+#include "hw.h"
+#include <math.h>
+
+// Settings
+#define PEDAL_INPUT_TIMEOUT				0.2
+#define MIN_MS_WITHOUT_POWER			500
+#define FILTER_SAMPLES					5
+#define RPM_FILTER_SAMPLES				8
+
+// Threads
+static THD_FUNCTION(pas_thread, arg);
+static THD_WORKING_AREA(pas_thread_wa, 1024);
+
+// Private variables
+static volatile pas_config config;
+static volatile float output_current_rel = 0.0;
+static volatile float ms_without_power = 0.0;
+static volatile float max_pulse_period = 0.0;
+static volatile float min_pedal_period = 0.0;
+static volatile float direction_conf = 0.0;
+static volatile float pedal_rpm = 0;
+static volatile bool primary_output = false;
+static volatile bool stop_now = true;
+static volatile bool is_running = false;
+
+void app_pas_configure(pas_config *conf) {
+	config = *conf;
+	ms_without_power = 0.0;
+	output_current_rel = 0.0;
+
+	// a period longer than this should immediately reduce power to zero
+	max_pulse_period = 1.0 / ((config.pedal_rpm_start / 60.0) * config.magnets) * 1.2;
+
+	// if pedal spins at x3 the end rpm, assume its beyond limits
+	min_pedal_period = 1.0 / ((config.pedal_rpm_end * 3.0 / 60.0));
+
+	if (config.invert_pedal_direction == true )
+		direction_conf= -1.0;
+	else
+		direction_conf = 1.0;
+}
+
+/**
+ * Start PAS thread
+ *
+ * @param is_primary_output
+ * True when PAS app takes direct control of the current target,
+ * false when PAS app shares control with the ADC app for current command
+ */
+void app_pas_start(bool is_primary_output) {
+	stop_now = false;
+	chThdCreateStatic(pas_thread_wa, sizeof(pas_thread_wa), NORMALPRIO, pas_thread, NULL);
+
+	primary_output = is_primary_output;
+}
+
+bool app_pas_is_running(void) {
+	return is_running;
+}
+
+void app_pas_stop(void) {
+	stop_now = true;
+	while (is_running) {
+		chThdSleepMilliseconds(1);
+	}
+
+	if (primary_output == true) {
+		mc_interface_set_current_rel(0.0);
+	}
+	else {
+		output_current_rel = 0.0;
+	}
+}
+
+float app_pas_get_current_target_rel(void) {
+	return output_current_rel;
+}
+
+void pas_event_handler(void) {
+#ifdef HW_PAS1_PORT
+	const int8_t QEM[] = {0,-1,1,2,1,0,2,-1,-1,2,0,1,2,1,-1,0}; // Quadrature Encoder Matrix
+	float direction_qem;
+	uint8_t new_state;
+	static uint8_t old_state = 0;
+	static float old_timestamp = 0;
+	static float inactivity_time = 0;
+	static float period_filtered = 0;
+
+	uint8_t PAS1_level = palReadPad(HW_PAS1_PORT, HW_PAS1_PIN);
+	uint8_t PAS2_level = palReadPad(HW_PAS2_PORT, HW_PAS2_PIN);
+
+	new_state = PAS2_level * 2 + PAS1_level;
+	direction_qem = (float) QEM[old_state * 4 + new_state];
+	old_state = new_state;
+
+	const float timestamp = (float)chVTGetSystemTimeX() / (float)CH_CFG_ST_FREQUENCY;
+
+	// sensors are poorly placed, so use only one rising edge as reference
+	if(new_state == 3) {
+		float period = (timestamp - old_timestamp) * (float)config.magnets;
+		old_timestamp = timestamp;
+
+		UTILS_LP_FAST(period_filtered, period, 1.0);
+
+		if(period_filtered < min_pedal_period) { //can't be that short, abort
+			return;
+		}
+		pedal_rpm = 60.0 / period_filtered;
+		pedal_rpm *= (direction_conf * direction_qem);
+		inactivity_time = 0.0;
+	}
+	else {
+		inactivity_time += 1.0 / (float)config.update_rate_hz;
+
+		//if no pedal activity, set RPM as zero
+		if(inactivity_time > max_pulse_period) {
+			pedal_rpm = 0.0;
+		}
+	}
+#endif
+}
+
+static THD_FUNCTION(pas_thread, arg) {
+	(void)arg;
+
+	float output = 0;
+	chRegSetThreadName("APP_PAS");
+
+#ifdef HW_PAS1_PORT
+	palSetPadMode(HW_PAS1_PORT, HW_PAS1_PIN, PAL_MODE_INPUT_PULLUP);
+	palSetPadMode(HW_PAS2_PORT, HW_PAS2_PIN, PAL_MODE_INPUT_PULLUP);
+#endif
+
+	is_running = true;
+
+	for(;;) {
+		// Sleep for a time according to the specified rate
+		systime_t sleep_time = CH_CFG_ST_FREQUENCY / config.update_rate_hz;
+
+		// At least one tick should be slept to not block the other threads
+		if (sleep_time == 0) {
+			sleep_time = 1;
+		}
+		chThdSleep(sleep_time);
+
+		if (stop_now) {
+			is_running = false;
+			return;
+		}
+
+		pas_event_handler();	// this should happen inside an ISR instead of being polled
+
+		// For safe start when fault codes occur
+		if (mc_interface_get_fault() != FAULT_CODE_NONE) {
+			ms_without_power = 0;
+		}
+
+		if (app_is_output_disabled()) {
+			continue;
+		}
+
+		// Map the rpm to assist level
+		switch (config.ctrl_type) {
+			case PAS_CTRL_TYPE_NONE:
+				output = 0.0;
+				break;
+			case PAS_CTRL_TYPE_CADENCE:
+				output = utils_map(pedal_rpm, config.pedal_rpm_start, config.pedal_rpm_end, 0.0, config.current_scaling);
+				utils_truncate_number(&output, 0.0, config.current_scaling);
+				break;
+			default:
+				break;
+		}
+
+		// Apply ramping
+		static systime_t last_time = 0;
+		static float output_ramp = 0.0;
+		float ramp_time = fabsf(output) > fabsf(output_ramp) ? config.ramp_time_pos : config.ramp_time_neg;
+
+		if (ramp_time > 0.01) {
+			const float ramp_step = (float)ST2MS(chVTTimeElapsedSinceX(last_time)) / (ramp_time * 1000.0);
+			utils_step_towards(&output_ramp, output, ramp_step);
+			utils_truncate_number(&output_ramp, 0.0, config.current_scaling);
+
+			last_time = chVTGetSystemTimeX();
+			output = output_ramp;
+		}
+
+		if (output < 0.001) {
+			ms_without_power += (1000.0 * (float)sleep_time) / (float)CH_CFG_ST_FREQUENCY;
+		}
+
+		// Safe start is enabled if the output has not been zero for long enough
+		if (ms_without_power < MIN_MS_WITHOUT_POWER) {
+			static int pulses_without_power_before = 0;
+			if (ms_without_power == pulses_without_power_before) {
+				ms_without_power = 0;
+			}
+			pulses_without_power_before = ms_without_power;
+			output_current_rel = 0.0;
+			continue;
+		}
+
+		// Reset timeout
+		timeout_reset();
+
+		if (primary_output == true) {
+			mc_interface_set_current_rel(output);
+		}
+		else {
+			output_current_rel = output;
+		}
+	}
+}

applications/applications.mk

@@ -5,6 +5,7 @@ APPSRC =	applications/app.c \
 			applications/app_uartcomm.c \
 			applications/app_nunchuk.c \
 			applications/app_balance.c \
+			applications/app_pas.c \
 			applications/app_custom.c
 
 APPINC = applications