From 33b5d5c1ba4f6eeb103370c207542efcfb288cda Mon Sep 17 00:00:00 2001 From: Marcos Chaparro Date: Mon, 7 Dec 2020 16:00:25 -0300 Subject: [PATCH] Pedal assist support (PAS) This commit enables cadence-based pedal assist for ebikes using quadrature-style pedal speed sensors. There are 2 operation modes: * PAS only: Motor current is based only on pedal feedback * ADC + PAS: The code will use both ADC and pedal feedback and use the strongest command to provide seamless overlap when the user is pedalling and requests extra torque with the throttle. Signed-off-by: Marcos Chaparro --- appconf/appconf_default.h | 38 ++++++ applications/app.c | 11 ++ applications/app.h | 6 + applications/app_adc.c | 4 + applications/app_pas.c | 243 +++++++++++++++++++++++++++++++++++ applications/applications.mk | 1 + confgenerator.c | 33 +++++ confgenerator.h | 4 +- datatypes.h | 32 ++++- 9 files changed, 369 insertions(+), 3 deletions(-) create mode 100644 applications/app_pas.c diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h index 290888b3..f03a9674 100644 --- a/appconf/appconf_default.h +++ b/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 diff --git a/applications/app.c b/applications/app.c index 1ba925db..cf7e51f2 100644 --- a/applications/app.c +++ b/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); diff --git a/applications/app.h b/applications/app.h index 6a0938d4..1e47e0b9 100644 --- a/applications/app.h +++ b/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); diff --git a/applications/app_adc.c b/applications/app_adc.c index 6428372a..d25d19e3 100644 --- a/applications/app_adc.c +++ b/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); diff --git a/applications/app_pas.c b/applications/app_pas.c new file mode 100644 index 00000000..5b69620b --- /dev/null +++ b/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 . + */ + +#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 + +// 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; + } + } +} diff --git a/applications/applications.mk b/applications/applications.mk index d211db10..25c85b19 100644 --- a/applications/applications.mk +++ b/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 diff --git a/confgenerator.c b/confgenerator.c index be7c5eed..f8c6d969 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -287,6 +287,17 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration buffer_append_float32_auto(buffer, conf->app_balance_conf.setpoint_target_filter, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.setpoint_filter_clamp, &ind); buffer_append_uint16(buffer, conf->app_balance_conf.kd_pt1_frequency, &ind); + buffer[ind++] = conf->app_pas_conf.ctrl_type; + buffer[ind++] = conf->app_pas_conf.sensor_type; + buffer_append_float32_auto(buffer, conf->app_pas_conf.current_scaling, &ind); + buffer_append_float32_auto(buffer, conf->app_pas_conf.pedal_rpm_start, &ind); + buffer_append_float32_auto(buffer, conf->app_pas_conf.pedal_rpm_end, &ind); + buffer[ind++] = conf->app_pas_conf.invert_pedal_direction; + buffer_append_uint16(buffer, conf->app_pas_conf.magnets, &ind); + buffer[ind++] = conf->app_pas_conf.use_filter; + buffer_append_float32_auto(buffer, conf->app_pas_conf.ramp_time_pos, &ind); + buffer_append_float32_auto(buffer, conf->app_pas_conf.ramp_time_neg, &ind); + buffer_append_uint16(buffer, conf->app_pas_conf.update_rate_hz, &ind); buffer[ind++] = conf->imu_conf.type; buffer[ind++] = conf->imu_conf.mode; buffer_append_uint16(buffer, conf->imu_conf.sample_rate_hz, &ind); @@ -600,6 +611,17 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration conf->app_balance_conf.setpoint_target_filter = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.setpoint_filter_clamp = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.kd_pt1_frequency = buffer_get_uint16(buffer, &ind); + conf->app_pas_conf.ctrl_type = buffer[ind++]; + conf->app_pas_conf.sensor_type = buffer[ind++]; + conf->app_pas_conf.current_scaling = buffer_get_float32_auto(buffer, &ind); + conf->app_pas_conf.pedal_rpm_start = buffer_get_float32_auto(buffer, &ind); + conf->app_pas_conf.pedal_rpm_end = buffer_get_float32_auto(buffer, &ind); + conf->app_pas_conf.invert_pedal_direction = buffer[ind++]; + conf->app_pas_conf.magnets = buffer_get_uint16(buffer, &ind); + conf->app_pas_conf.use_filter = buffer[ind++]; + conf->app_pas_conf.ramp_time_pos = buffer_get_float32_auto(buffer, &ind); + conf->app_pas_conf.ramp_time_neg = buffer_get_float32_auto(buffer, &ind); + conf->app_pas_conf.update_rate_hz = buffer_get_uint16(buffer, &ind); conf->imu_conf.type = buffer[ind++]; conf->imu_conf.mode = buffer[ind++]; conf->imu_conf.sample_rate_hz = buffer_get_uint16(buffer, &ind); @@ -897,6 +919,17 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) { conf->app_balance_conf.setpoint_target_filter = APPCONF_BALANCE_SETPOINT_TARGET_FILTER; conf->app_balance_conf.setpoint_filter_clamp = APPCONF_BALANCE_SETPOINT_FILTER_CLAMP; conf->app_balance_conf.kd_pt1_frequency = APPCONF_BALANCE_KD_PT1_FREQUENCY; + conf->app_pas_conf.ctrl_type = APPCONF_PAS_CTRL_TYPE; + conf->app_pas_conf.sensor_type = APPCONF_PAS_SENSOR_TYPE; + conf->app_pas_conf.current_scaling = APPCONF_PAS_CURRENT_SCALING; + conf->app_pas_conf.pedal_rpm_start = APPCONF_PAS_PEDAL_RPM_START; + conf->app_pas_conf.pedal_rpm_end = APPCONF_PAS_PEDAL_RPM_END; + conf->app_pas_conf.invert_pedal_direction = APPCONF_PAS_INVERT_PEDAL_DIRECTION; + conf->app_pas_conf.magnets = APPCONF_PAS_MAGNETS; + conf->app_pas_conf.use_filter = APPCONF_PAS_USE_FILTER; + conf->app_pas_conf.ramp_time_pos = APPCONF_PAS_RAMP_TIME_POS; + conf->app_pas_conf.ramp_time_neg = APPCONF_PAS_RAMP_TIME_NEG; + conf->app_pas_conf.update_rate_hz = APPCONF_PAS_UPDATE_RATE_HZ; conf->imu_conf.type = APPCONF_IMU_TYPE; conf->imu_conf.mode = APPCONF_IMU_AHRS_MODE; conf->imu_conf.sample_rate_hz = APPCONF_IMU_SAMPLE_RATE_HZ; diff --git a/confgenerator.h b/confgenerator.h index 17b7c91e..f5c1bf7f 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -8,8 +8,8 @@ #include // Constants -#define MCCONF_SIGNATURE 1050464887 -#define APPCONF_SIGNATURE 3725342318 +#define MCCONF_SIGNATURE 789840453 +#define APPCONF_SIGNATURE 4074252355 // Functions int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf); diff --git a/datatypes.h b/datatypes.h index bad98b4f..ba81f819 100644 --- a/datatypes.h +++ b/datatypes.h @@ -447,7 +447,9 @@ typedef enum { APP_NUNCHUK, APP_NRF, APP_CUSTOM, - APP_BALANCE + APP_BALANCE, + APP_PAS, + APP_ADC_PAS } app_use; // Throttle curve mode @@ -512,6 +514,17 @@ typedef enum { ADC_CTRL_TYPE_PID_REV_BUTTON } adc_control_type; +// PAS control types +typedef enum { + PAS_CTRL_TYPE_NONE = 0, + PAS_CTRL_TYPE_CADENCE +} pas_control_type; + +// PAS sensor types +typedef enum { + PAS_SENSOR_TYPE_QUADRATURE = 0 +} pas_sensor_type; + typedef struct { adc_control_type ctrl_type; float hyst; @@ -562,6 +575,20 @@ typedef struct { float smart_rev_ramp_time; } chuk_config; +typedef struct { + pas_control_type ctrl_type; + pas_sensor_type sensor_type; + float current_scaling; + float pedal_rpm_start; + float pedal_rpm_end; + bool invert_pedal_direction; + uint8_t magnets; + bool use_filter; + float ramp_time_pos; + float ramp_time_neg; + uint32_t update_rate_hz; +} pas_config; + // NRF Datatypes typedef enum { NRF_SPEED_250K = 0, @@ -765,6 +792,9 @@ typedef struct { // Balance application settings balance_config app_balance_conf; + // Pedal Assist application settings + pas_config app_pas_conf; + // IMU Settings imu_config imu_conf;