mirror of https://github.com/rusefi/bldc.git
Merge pull request #243 from powerdesigns/pedal_assist
Pedal assist support (PAS)
This commit is contained in:
Benjamin Vedder
GitHub
commit
a17c2f5453
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -8,8 +8,8 @@
|
|||
#include <stdbool.h>
|
||||
|
||||
// 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);
|
||||
|
|
|
|||
32
datatypes.h
32
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;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue