rusefi
/
bldc
mirror of https://github.com/rusefi/bldc.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
bldc/applications/app_ppm.c

518 lines
15 KiB
C
Raw Blame History

/*
Copyright 2016 - 2019 Benjamin Vedder benjamin@vedder.se
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 "servo_dec.h"
#include "mc_interface.h"
#include "timeout.h"
#include "utils.h"
#include "comm_can.h"
#include <math.h>
// Settings
#define MAX_CAN_AGE 0.1
#define MIN_PULSES_WITHOUT_POWER 50
// Threads
static THD_FUNCTION(ppm_thread, arg);
static THD_WORKING_AREA(ppm_thread_wa, 512);
static thread_t *ppm_tp;
static volatile bool ppm_rx = false;
// Private functions
static void servodec_func(void);
// Private variables
static volatile bool is_running = false;
static volatile bool stop_now = true;
static volatile ppm_config config;
static volatile int pulses_without_power = 0;
static float input_val = 0.0;
static volatile float direction_hyst = 0;
// Private functions
void app_ppm_configure(ppm_config *conf) {
config = *conf;
pulses_without_power = 0;
if (is_running) {
servodec_set_pulse_options(config.pulse_start, config.pulse_end, config.median_filter);
}
direction_hyst = config.max_erpm_for_dir * 0.20;
}
void app_ppm_start(void) {
stop_now = false;
chThdCreateStatic(ppm_thread_wa, sizeof(ppm_thread_wa), NORMALPRIO, ppm_thread, NULL);
}
void app_ppm_stop(void) {
stop_now = true;
if (is_running) {
chEvtSignalI(ppm_tp, (eventmask_t) 1);
servodec_stop();
}
while(is_running) {
chThdSleepMilliseconds(1);
}
}
float app_ppm_get_decoded_level(void) {
return input_val;
}
static void servodec_func(void) {
ppm_rx = true;
chSysLockFromISR();
chEvtSignalI(ppm_tp, (eventmask_t) 1);
chSysUnlockFromISR();
}
static THD_FUNCTION(ppm_thread, arg) {
(void)arg;
chRegSetThreadName("APP_PPM");
ppm_tp = chThdGetSelfX();
servodec_set_pulse_options(config.pulse_start, config.pulse_end, config.median_filter);
servodec_init(servodec_func);
is_running = true;
for(;;) {
chEvtWaitAnyTimeout((eventmask_t)1, MS2ST(2));
if (stop_now) {
is_running = false;
return;
}
if (ppm_rx) {
ppm_rx = false;
timeout_reset();
}
const volatile mc_configuration *mcconf = mc_interface_get_configuration();
const float rpm_now = mc_interface_get_rpm();
float servo_val = servodec_get_servo(0);
float servo_ms = utils_map(servo_val, -1.0, 1.0, config.pulse_start, config.pulse_end);
static bool servoError = false;
switch (config.ctrl_type) {
case PPM_CTRL_TYPE_CURRENT_NOREV:
case PPM_CTRL_TYPE_DUTY_NOREV:
case PPM_CTRL_TYPE_PID_NOREV:
input_val = servo_val;
servo_val += 1.0;
servo_val /= 2.0;
break;
default:
// Mapping with respect to center pulsewidth
if (servo_ms < config.pulse_center) {
servo_val = utils_map(servo_ms, config.pulse_start,
config.pulse_center, -1.0, 0.0);
} else {
servo_val = utils_map(servo_ms, config.pulse_center,
config.pulse_end, 0.0, 1.0);
}
input_val = servo_val;
break;
}
// All pins and buttons are still decoded for debugging, even
// when output is disabled.
if (app_is_output_disabled()) {
continue;
}
if (timeout_has_timeout() || servodec_get_time_since_update() > timeout_get_timeout_msec()) {
pulses_without_power = 0;
servoError = true;
float timeoutCurrent = timeout_get_brake_current();
mc_interface_set_brake_current(timeoutCurrent);
if(config.multi_esc){
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
comm_can_set_current_brake(msg->id, timeoutCurrent);
}
}
}
continue;
} else if (mc_interface_get_fault() != FAULT_CODE_NONE && config.safe_start != SAFE_START_NO_FAULT){
pulses_without_power = 0;
}
// Apply deadband
utils_deadband(&servo_val, config.hyst, 1.0);
// Apply throttle curve
servo_val = utils_throttle_curve(servo_val, config.throttle_exp, config.throttle_exp_brake, config.throttle_exp_mode);
// Apply ramping
static systime_t last_time = 0;
static float servo_val_ramp = 0.0;
float ramp_time = fabsf(servo_val) > fabsf(servo_val_ramp) ? config.ramp_time_pos : config.ramp_time_neg;
// TODO: Remember what this was about?
// if (fabsf(servo_val) > 0.001) {
// ramp_time = fminf(config.ramp_time_pos, config.ramp_time_neg);
// }
const float dt = (float)ST2MS(chVTTimeElapsedSinceX(last_time)) / 1000.0;
last_time = chVTGetSystemTimeX();
if (ramp_time > 0.01) {
const float ramp_step = dt / ramp_time;
utils_step_towards(&servo_val_ramp, servo_val, ramp_step);
servo_val = servo_val_ramp;
}
float current = 0;
bool current_mode = false;
bool current_mode_brake = false;
bool send_current = false;
bool send_duty = false;
static bool force_brake = true;
static int8_t did_idle_once = 0; //0 = haven't idle ;1 = idle once ; 2 = idle twice
float rpm_local = mc_interface_get_rpm();
float rpm_lowest = rpm_local;
float rpm_highest = rpm_local;
switch (config.ctrl_type) {
case PPM_CTRL_TYPE_CURRENT_BRAKE_REV_HYST:
current_mode = true;
// Hysteresis 20 % of actual RPM
if (force_brake) {
if (rpm_local < config.max_erpm_for_dir - direction_hyst) { // for 2500 it's 2000
force_brake = false;
did_idle_once = 0;
}
} else {
if (rpm_local > config.max_erpm_for_dir + direction_hyst) { // for 2500 it's 3000
force_brake = true;
did_idle_once = 0;
}
}
if (servo_val >= 0.0) {
if (servo_val == 0.0) {
// if there was a idle in between then allow going backwards
if (did_idle_once == 1 && !force_brake) {
did_idle_once = 2;
}
} else{
// accelerated forward or fast enough at least
if (rpm_local > -config.max_erpm_for_dir){ // for 2500 it's -2500
did_idle_once = 0;
}
}
if (rpm_now >= 0.0) { //Accelerate
current = servo_val * mcconf->lo_current_motor_max_now;
} else { //Brake
current = servo_val * fabsf(mcconf->lo_current_motor_min_now);
}
} else {
// too fast
if (force_brake){
current_mode_brake = true;
} else {
// not too fast backwards
if (rpm_local > -config.max_erpm_for_dir) { // for 2500 it's -2500
// first time that we brake and we are not too fast
if (did_idle_once != 2) {
did_idle_once = 1;
current_mode_brake = true;
}
// too fast backwards
} else {
// if brake was active already
if (did_idle_once == 1) {
current_mode_brake = true;
} else {
// it's ok to go backwards now braking would be strange now
did_idle_once = 2;
}
}
}
if (current_mode_brake) {
// braking
current = fabsf(servo_val * mcconf->lo_current_motor_min_now);
} else {
// reverse acceleration
current = servo_val * fabsf(mcconf->lo_current_motor_min_now);
}
}
if (fabsf(servo_val) < 0.001) {
pulses_without_power++;
}
break;
case PPM_CTRL_TYPE_CURRENT:
case PPM_CTRL_TYPE_CURRENT_NOREV:
current_mode = true;
if ((servo_val >= 0.0 && rpm_now >= 0.0) || (servo_val < 0.0 && rpm_now <= 0.0)) { //Accelerate
current = servo_val * mcconf->lo_current_motor_max_now;
} else { //Brake
current = servo_val * fabsf(mcconf->lo_current_motor_min_now);
}
if (fabsf(servo_val) < 0.001) {
pulses_without_power++;
}
break;
case PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE:
case PPM_CTRL_TYPE_CURRENT_SMART_REV:
current_mode = true;
current_mode_brake = servo_val < 0.0;
if (servo_val >= 0.0 && rpm_now > 0.0) { //Positive input AND going forward = accelerating
current = servo_val * mcconf->lo_current_motor_max_now;
} else { //Negative input OR going backwards = brake (no reverse allowed in those control types)
current = fabsf(servo_val * mcconf->lo_current_motor_min_now);
}
if (fabsf(servo_val) < 0.001) {
pulses_without_power++;
}
break;
case PPM_CTRL_TYPE_DUTY:
case PPM_CTRL_TYPE_DUTY_NOREV:
if (fabsf(servo_val) < 0.001) {
pulses_without_power++;
}
if (!(pulses_without_power < MIN_PULSES_WITHOUT_POWER && config.safe_start)) {
mc_interface_set_duty(utils_map(servo_val, -1.0, 1.0, -mcconf->l_max_duty, mcconf->l_max_duty));
send_duty = true;
}
break;
case PPM_CTRL_TYPE_PID:
case PPM_CTRL_TYPE_PID_NOREV:
if (fabsf(servo_val) < 0.001) {
pulses_without_power++;
}
if (!(pulses_without_power < MIN_PULSES_WITHOUT_POWER && config.safe_start)) {
mc_interface_set_pid_speed(servo_val * config.pid_max_erpm);
send_current = true;
}
break;
default:
continue;
}
//Safe start : If startup, servo timeout or fault, check if idle has been verified for some pulses before driving the motor
if (pulses_without_power < MIN_PULSES_WITHOUT_POWER && config.safe_start) {
static int pulses_without_power_before = 0;
if (pulses_without_power == pulses_without_power_before) {
pulses_without_power = 0;
}
pulses_without_power_before = pulses_without_power;
if (servoError){
continue;
}
if (current_mode) {
current = 0.0;
}
} else {
servoError = false;
}
const float duty_now = mc_interface_get_duty_cycle_now();
float current_highest_abs = fabsf(mc_interface_get_tot_current_directional_filtered());
float duty_highest_abs = fabsf(duty_now);
//If multiple VESCs over CAN, store highest/lowest running values of the whole setup
if (config.multi_esc) {
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
if (fabsf(msg->rpm) < fabsf(rpm_lowest)) {
rpm_lowest = msg->rpm;
}
if (fabsf(msg->rpm) > fabsf(rpm_highest)) {
rpm_highest = msg->rpm;
}
if (fabsf(msg->current) > current_highest_abs) {
current_highest_abs = fabsf(msg->current);
}
if (fabsf(msg->duty) > duty_highest_abs) {
duty_highest_abs = fabsf(msg->duty);
}
}
}
}
if (config.ctrl_type == PPM_CTRL_TYPE_CURRENT_SMART_REV) {
bool duty_control = false;
static bool was_duty_control = false;
static float duty_rev = 0.0;
if (servo_val < -0.92 && duty_highest_abs < (mcconf->l_min_duty * 1.5) &&
current_highest_abs < (mcconf->l_current_max * mcconf->l_current_max_scale * 0.7)) {
duty_control = true;
}
if (duty_control || (was_duty_control && servo_val < -0.1)) {
was_duty_control = true;
float goal = config.smart_rev_max_duty * -servo_val;
utils_step_towards(&duty_rev, -goal,
config.smart_rev_max_duty * dt / config.smart_rev_ramp_time);
mc_interface_set_duty(duty_rev);
// Send the same duty cycle to the other controllers
if (config.multi_esc) {
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
comm_can_set_duty(msg->id, duty_rev);
}
}
}
current_mode = false;
} else {
duty_rev = duty_now;
was_duty_control = false;
}
}
//CTRL TYPE PID_NOREV & DUTY_NOREV : Acting as master, send motor control command to all slave VESCs detected on the CANbus
if ((send_current || send_duty) && config.multi_esc) {
float current_filtered = mc_interface_get_tot_current_directional_filtered();
float duty = mc_interface_get_duty_cycle_now();
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
if (send_current) {
comm_can_set_current(msg->id, current_filtered);
} else if (send_duty) {
comm_can_set_duty(msg->id, duty);
}
}
}
}
//CTRL TYPE CURRENT
if (current_mode) {
if (current_mode_brake) { //If braking applied
mc_interface_set_brake_current(fabsf(current));
// Send brake command to all ESCs seen recently on the CAN bus
if (config.multi_esc) {
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
comm_can_set_current_brake_rel(msg->id, fabsf(servo_val));
}
}
}
} else {
float current_out = current;
bool is_reverse = false;
static bool autoTCdisengaged = false;
if (current_out < 0.0) { // Not braking AND negative current = reverse engaged
is_reverse = true;
current_out = -current_out;
current = -current;
rpm_local = -rpm_local;
rpm_lowest = -rpm_lowest;
rpm_highest = -rpm_highest;
servo_val = -servo_val;
}
// Send acceleration command to all ESCs seen recently on the CAN bus
if (config.multi_esc) {
if (config.tc) {
if(mc_interface_get_fault() != FAULT_CODE_NONE) {
autoTCdisengaged = true;
} else if (autoTCdisengaged && rpm_highest < rpm_local + config.tc_max_diff && rpm_lowest > rpm_local - config.tc_max_diff) { //No Fault anymore and no traction control action needed = re-enable traction control
autoTCdisengaged = false;
}
}
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
//Traction Control - Applied to slaves except if a fault has occured on the local VESC (undriven wheel may generate fake RPM)
if (config.tc && config.tc_max_diff > 1.0 && !autoTCdisengaged) {
float rpm_tmp = msg->rpm;
if (is_reverse) {
rpm_tmp = -rpm_tmp;
}
float diff = rpm_tmp - rpm_lowest;
servo_val = utils_map(diff, 0.0, config.tc_max_diff, servo_val, 0.0);
}
//Send motor drive command to slaves
if (is_reverse) {
comm_can_set_current_rel(msg->id, -servo_val);
} else {
comm_can_set_current_rel(msg->id, servo_val);
}
}
}
//Traction Control - Applying locally
if (config.tc && config.tc_max_diff > 1.0) {
float diff = rpm_local - rpm_lowest;
current_out = utils_map(diff, 0.0, config.tc_max_diff, current, 0.0);
if (current_out < mcconf->cc_min_current) {
current_out = 0.0;
}
}
}
//Drive local motor
if (is_reverse) {
mc_interface_set_current(-current_out);
} else {
mc_interface_set_current(current_out);
}
}
}
}
}
Reference in New Issue View Git Blame Copy Permalink