2014-04-18 15:09:46 -07:00
/*
2019-07-01 09:35:27 -07:00
Copyright 2016 - 2019 Benjamin Vedder benjamin @ vedder . se
2014-04-18 15:09:46 -07:00
2016-11-04 07:18:34 -07:00
This file is part of the VESC firmware .
The VESC firmware is free software : you can redistribute it and / or modify
2014-04-18 15:09:46 -07:00
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 .
2016-11-04 07:18:34 -07:00
The VESC firmware is distributed in the hope that it will be useful ,
2014-04-18 15:09:46 -07:00
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/>.
2016-11-04 07:18:34 -07:00
*/
2014-04-18 15:09:46 -07:00
# include "app.h"
# include "ch.h"
# include "hal.h"
# include "stm32f4xx_conf.h"
# include "servo_dec.h"
2015-12-08 12:01:23 -08:00
# include "mc_interface.h"
2014-09-20 04:41:18 -07:00
# include "timeout.h"
2014-09-21 10:29:26 -07:00
# include "utils.h"
2015-02-19 12:20:07 -08:00
# include "comm_can.h"
2014-04-18 15:09:46 -07:00
# include <math.h>
2015-07-31 14:26:50 -07:00
// Only available if servo output is not active
# if !SERVO_OUT_ENABLE
2015-02-19 12:20:07 -08:00
// Settings
# define MAX_CAN_AGE 0.1
2015-04-30 16:57:55 -07:00
# define MIN_PULSES_WITHOUT_POWER 50
2015-02-19 12:20:07 -08:00
2014-04-18 15:09:46 -07:00
// Threads
2015-10-08 14:09:39 -07:00
static THD_FUNCTION ( ppm_thread , arg ) ;
2019-12-19 07:55:38 -08:00
static THD_WORKING_AREA ( ppm_thread_wa , 1536 ) ;
2015-10-08 14:09:39 -07:00
static thread_t * ppm_tp ;
2019-12-19 07:55:38 -08:00
static volatile bool ppm_rx = false ;
2014-04-18 15:09:46 -07:00
// Private functions
static void servodec_func ( void ) ;
2014-09-17 12:05:57 -07:00
// Private variables
2014-09-19 18:22:38 -07:00
static volatile bool is_running = false ;
2016-11-04 07:18:34 -07:00
static volatile bool stop_now = true ;
2015-02-19 12:20:07 -08:00
static volatile ppm_config config ;
2015-04-30 16:57:55 -07:00
static volatile int pulses_without_power = 0 ;
2017-09-04 12:12:43 -07:00
static float input_val = 0.0 ;
2019-06-19 19:00:46 -07:00
static volatile float direction_hyst = 0 ;
2014-09-19 18:22:38 -07:00
2014-09-21 10:29:26 -07:00
// Private functions
2015-07-31 14:26:50 -07:00
# endif
2014-09-21 10:29:26 -07:00
2015-02-19 12:20:07 -08:00
void app_ppm_configure ( ppm_config * conf ) {
2015-07-31 14:26:50 -07:00
# if !SERVO_OUT_ENABLE
2015-02-19 12:20:07 -08:00
config = * conf ;
2015-04-30 16:57:55 -07:00
pulses_without_power = 0 ;
2014-09-19 18:22:38 -07:00
if ( is_running ) {
2015-04-30 16:57:55 -07:00
servodec_set_pulse_options ( config . pulse_start , config . pulse_end , config . median_filter ) ;
2014-09-19 18:22:38 -07:00
}
2019-06-19 19:00:46 -07:00
direction_hyst = config . max_erpm_for_dir * 0.20 ;
2015-07-31 14:26:50 -07:00
# else
( void ) conf ;
# endif
2014-09-17 12:05:57 -07:00
}
void app_ppm_start ( void ) {
2015-07-31 14:26:50 -07:00
# if !SERVO_OUT_ENABLE
2016-11-04 07:18:34 -07:00
stop_now = false ;
2014-09-17 12:05:57 -07:00
chThdCreateStatic ( ppm_thread_wa , sizeof ( ppm_thread_wa ) , NORMALPRIO , ppm_thread , NULL ) ;
2015-07-31 14:26:50 -07:00
# endif
2014-05-04 07:05:10 -07:00
}
2016-11-04 07:18:34 -07:00
void app_ppm_stop ( void ) {
# if !SERVO_OUT_ENABLE
stop_now = true ;
if ( is_running ) {
chEvtSignalI ( ppm_tp , ( eventmask_t ) 1 ) ;
servodec_stop ( ) ;
}
while ( is_running ) {
chThdSleepMilliseconds ( 1 ) ;
}
# endif
}
2017-09-04 12:12:43 -07:00
float app_ppm_get_decoded_level ( void ) {
# if !SERVO_OUT_ENABLE
return input_val ;
# else
return 0.0 ;
# endif
}
2015-07-31 14:26:50 -07:00
# if !SERVO_OUT_ENABLE
2014-04-18 15:09:46 -07:00
static void servodec_func ( void ) {
2019-12-19 07:55:38 -08:00
ppm_rx = true ;
2015-10-08 14:09:39 -07:00
chSysLockFromISR ( ) ;
2014-09-17 12:05:57 -07:00
chEvtSignalI ( ppm_tp , ( eventmask_t ) 1 ) ;
2015-10-08 14:09:39 -07:00
chSysUnlockFromISR ( ) ;
2014-04-18 15:09:46 -07:00
}
2015-10-08 14:09:39 -07:00
static THD_FUNCTION ( ppm_thread , arg ) {
2014-04-18 15:09:46 -07:00
( void ) arg ;
2014-09-17 12:05:57 -07:00
chRegSetThreadName ( " APP_PPM " ) ;
2015-10-08 14:09:39 -07:00
ppm_tp = chThdGetSelfX ( ) ;
2014-04-18 15:09:46 -07:00
2015-04-30 16:57:55 -07:00
servodec_set_pulse_options ( config . pulse_start , config . pulse_end , config . median_filter ) ;
2014-05-10 01:21:47 -07:00
servodec_init ( servodec_func ) ;
2014-09-19 18:22:38 -07:00
is_running = true ;
2014-04-18 15:09:46 -07:00
for ( ; ; ) {
2019-12-22 12:22:07 -08:00
chEvtWaitAnyTimeout ( ( eventmask_t ) 1 , MS2ST ( 2 ) ) ;
2014-04-18 15:09:46 -07:00
2016-11-04 07:18:34 -07:00
if ( stop_now ) {
is_running = false ;
return ;
}
2019-12-19 07:55:38 -08:00
if ( ppm_rx ) {
ppm_rx = false ;
timeout_reset ( ) ;
}
2017-09-04 12:12:43 -07:00
const volatile mc_configuration * mcconf = mc_interface_get_configuration ( ) ;
const float rpm_now = mc_interface_get_rpm ( ) ;
2014-09-20 04:41:18 -07:00
float servo_val = servodec_get_servo ( 0 ) ;
2017-09-04 12:12:43 -07:00
float servo_ms = utils_map ( servo_val , - 1.0 , 1.0 , config . pulse_start , config . pulse_end ) ;
2020-07-03 06:16:46 -07:00
static bool servoError = false ;
2014-09-17 12:05:57 -07:00
2015-02-19 12:20:07 -08:00
switch ( config . ctrl_type ) {
2014-09-20 04:41:18 -07:00
case PPM_CTRL_TYPE_CURRENT_NOREV :
case PPM_CTRL_TYPE_DUTY_NOREV :
case PPM_CTRL_TYPE_PID_NOREV :
2017-09-04 12:12:43 -07:00
input_val = servo_val ;
2014-09-20 04:41:18 -07:00
servo_val + = 1.0 ;
servo_val / = 2.0 ;
break ;
2014-09-18 14:00:14 -07:00
2014-09-20 04:41:18 -07:00
default :
2017-09-04 12:12:43 -07:00
// 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 ;
2014-09-20 04:41:18 -07:00
break ;
}
2014-09-17 12:05:57 -07:00
2019-02-18 10:30:19 -08:00
// All pins and buttons are still decoded for debugging, even
// when output is disabled.
if ( app_is_output_disabled ( ) ) {
continue ;
}
2020-07-03 06:16:46 -07:00
if ( timeout_has_timeout ( ) | | servodec_get_time_since_update ( ) > timeout_get_timeout_msec ( ) ) {
2017-09-04 12:12:43 -07:00
pulses_without_power = 0 ;
2020-07-03 06:16:46 -07:00
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 ) ;
}
}
}
2017-09-04 12:12:43 -07:00
continue ;
2020-07-03 06:16:46 -07:00
} else if ( mc_interface_get_fault ( ) ! = FAULT_CODE_NONE ) {
pulses_without_power = 0 ;
2017-09-04 12:12:43 -07:00
}
// Apply deadband
2015-02-19 12:20:07 -08:00
utils_deadband ( & servo_val , config . hyst , 1.0 ) ;
2017-09-04 12:12:43 -07:00
// Apply throttle curve
2017-09-06 12:13:28 -07:00
servo_val = utils_throttle_curve ( servo_val , config . throttle_exp , config . throttle_exp_brake , config . throttle_exp_mode ) ;
2017-09-04 12:12:43 -07:00
// Apply ramping
static systime_t last_time = 0 ;
static float servo_val_ramp = 0.0 ;
2019-03-01 08:18:07 -08:00
float ramp_time = fabsf ( servo_val ) > fabsf ( servo_val_ramp ) ? config . ramp_time_pos : config . ramp_time_neg ;
2019-12-19 07:55:38 -08:00
// TODO: Remember what this was about?
// if (fabsf(servo_val) > 0.001) {
// ramp_time = fminf(config.ramp_time_pos, config.ramp_time_neg);
// }
2017-09-04 12:12:43 -07:00
2019-09-09 10:34:26 -07:00
const float dt = ( float ) ST2MS ( chVTTimeElapsedSinceX ( last_time ) ) / 1000.0 ;
last_time = chVTGetSystemTimeX ( ) ;
2017-09-04 12:12:43 -07:00
if ( ramp_time > 0.01 ) {
2019-09-09 10:34:26 -07:00
const float ramp_step = dt / ramp_time ;
2017-09-04 12:12:43 -07:00
utils_step_towards ( & servo_val_ramp , servo_val , ramp_step ) ;
servo_val = servo_val_ramp ;
}
2014-09-21 10:29:26 -07:00
float current = 0 ;
bool current_mode = false ;
bool current_mode_brake = false ;
2017-09-04 12:12:43 -07:00
bool send_current = false ;
2019-04-18 11:00:26 -07:00
bool send_duty = false ;
2019-06-24 13:36:35 -07:00
static bool force_brake = true ;
2020-07-03 06:16:46 -07:00
static int8_t did_idle_once = 0 ; //0 = haven't idle ;1 = idle once ; 2 = idle twice
2019-06-24 13:36:35 -07:00
float rpm_local = mc_interface_get_rpm ( ) ;
float rpm_lowest = rpm_local ;
2020-07-03 06:16:46 -07:00
float rpm_highest = rpm_local ;
2014-09-21 10:29:26 -07:00
2015-02-19 12:20:07 -08:00
switch ( config . ctrl_type ) {
2019-06-24 13:36:35 -07:00
case PPM_CTRL_TYPE_CURRENT_BRAKE_REV_HYST :
2019-07-01 09:35:27 -07:00
current_mode = true ;
2019-06-24 13:36:35 -07:00
// 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 ;
}
2019-07-01 10:29:09 -07:00
} else {
2019-06-24 13:36:35 -07:00
// accelerated forward or fast enough at least
if ( rpm_local > - config . max_erpm_for_dir ) { // for 2500 it's -2500
did_idle_once = 0 ;
}
}
current = servo_val * mcconf - > lo_current_motor_max_now ;
} else {
// too fast
if ( force_brake ) {
current_mode_brake = true ;
2020-07-03 06:16:46 -07:00
} else {
2019-06-24 13:36:35 -07:00
// 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 ) ;
2019-07-01 09:35:27 -07:00
} else {
2019-06-24 13:36:35 -07:00
// reverse acceleration
current = servo_val * fabsf ( mcconf - > lo_current_motor_min_now ) ;
}
}
2019-06-28 15:45:28 -07:00
if ( fabsf ( servo_val ) < 0.001 ) {
pulses_without_power + + ;
}
2019-06-24 13:36:35 -07:00
break ;
2014-09-20 04:41:18 -07:00
case PPM_CTRL_TYPE_CURRENT :
case PPM_CTRL_TYPE_CURRENT_NOREV :
2014-09-21 10:29:26 -07:00
current_mode = true ;
2020-07-03 06:16:46 -07:00
if ( ( servo_val > = 0.0 & & rpm_now > = 0.0 ) | | ( servo_val < 0.0 & & rpm_now < = 0.0 ) ) { //Accelerate
2017-09-04 12:12:43 -07:00
current = servo_val * mcconf - > lo_current_motor_max_now ;
2020-07-03 06:16:46 -07:00
} else { //Brake
2017-09-04 12:12:43 -07:00
current = servo_val * fabsf ( mcconf - > lo_current_motor_min_now ) ;
2014-04-18 15:09:46 -07:00
}
2015-04-30 16:57:55 -07:00
if ( fabsf ( servo_val ) < 0.001 ) {
pulses_without_power + + ;
}
2014-09-20 04:41:18 -07:00
break ;
2014-04-18 15:09:46 -07:00
2014-09-20 04:41:18 -07:00
case PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE :
2019-09-09 10:34:26 -07:00
case PPM_CTRL_TYPE_CURRENT_SMART_REV :
2014-09-21 10:29:26 -07:00
current_mode = true ;
2020-04-27 05:23:49 -07:00
current_mode_brake = servo_val < 0.0 ;
2020-07-03 06:16:46 -07:00
if ( servo_val > = 0.0 & & rpm_now > 0.0 ) { //Positive input AND going forward = accelerating
2017-09-04 12:12:43 -07:00
current = servo_val * mcconf - > lo_current_motor_max_now ;
2020-07-03 06:16:46 -07:00
} else { //Negative input OR going backwards = brake (no reverse allowed in those control types)
2017-09-04 12:12:43 -07:00
current = fabsf ( servo_val * mcconf - > lo_current_motor_min_now ) ;
2014-09-19 18:22:38 -07:00
}
2015-04-30 16:57:55 -07:00
2018-02-27 16:10:27 -08:00
if ( fabsf ( servo_val ) < 0.001 ) {
2015-04-30 16:57:55 -07:00
pulses_without_power + + ;
}
2014-09-20 04:41:18 -07:00
break ;
case PPM_CTRL_TYPE_DUTY :
case PPM_CTRL_TYPE_DUTY_NOREV :
2015-04-30 16:57:55 -07:00
if ( fabsf ( servo_val ) < 0.001 ) {
pulses_without_power + + ;
}
if ( ! ( pulses_without_power < MIN_PULSES_WITHOUT_POWER & & config . safe_start ) ) {
2016-02-09 03:23:58 -08:00
mc_interface_set_duty ( utils_map ( servo_val , - 1.0 , 1.0 , - mcconf - > l_max_duty , mcconf - > l_max_duty ) ) ;
2019-04-18 11:00:26 -07:00
send_duty = true ;
2015-04-30 16:57:55 -07:00
}
2014-09-20 04:41:18 -07:00
break ;
case PPM_CTRL_TYPE_PID :
case PPM_CTRL_TYPE_PID_NOREV :
2015-04-30 16:57:55 -07:00
if ( fabsf ( servo_val ) < 0.001 ) {
pulses_without_power + + ;
}
if ( ! ( pulses_without_power < MIN_PULSES_WITHOUT_POWER & & config . safe_start ) ) {
2015-12-08 12:01:23 -08:00
mc_interface_set_pid_speed ( servo_val * config . pid_max_erpm ) ;
2017-09-04 12:12:43 -07:00
send_current = true ;
2015-04-30 16:57:55 -07:00
}
2014-09-20 04:41:18 -07:00
break ;
default :
2015-04-30 16:57:55 -07:00
continue ;
}
2020-07-03 06:16:46 -07:00
//Safe start : If startup, servo timeout or fault, check if idle has been verified for some pulses before driving the motor
2015-04-30 16:57:55 -07:00
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 ;
2020-07-03 06:16:46 -07:00
if ( servoError ) {
continue ;
}
if ( current_mode ) {
current = 0.0 ;
}
} else {
servoError = false ;
2015-04-30 16:57:55 -07:00
}
2019-09-09 10:34:26 -07:00
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 ) ;
2020-07-03 06:16:46 -07:00
//If multiple VESCs over CAN, store highest/lowest running values of the whole setup
2015-04-30 16:57:55 -07:00
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 ) {
2019-09-09 10:34:26 -07:00
if ( fabsf ( msg - > rpm ) < fabsf ( rpm_lowest ) ) {
rpm_lowest = msg - > rpm ;
}
2020-07-03 06:16:46 -07:00
if ( fabsf ( msg - > rpm ) > fabsf ( rpm_highest ) ) {
rpm_highest = msg - > rpm ;
}
2019-09-09 10:34:26 -07:00
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 ) ;
}
}
}
}
2015-04-30 16:57:55 -07:00
2019-09-09 10:34:26 -07:00
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 ) ;
}
2015-04-30 16:57:55 -07:00
}
}
2019-09-09 10:34:26 -07:00
current_mode = false ;
} else {
duty_rev = duty_now ;
was_duty_control = false ;
2015-04-30 16:57:55 -07:00
}
2014-04-18 15:09:46 -07:00
}
2020-07-03 06:16:46 -07:00
//CTRL TYPE PID_NOREV & DUTY_NOREV : Acting as master, send motor control command to all slave VESCs detected on the CANbus
2019-04-18 11:00:26 -07:00
if ( ( send_current | | send_duty ) & & config . multi_esc ) {
2020-07-03 06:16:46 -07:00
float current_filtered = mc_interface_get_tot_current_directional_filtered ( ) ;
2019-04-18 11:00:26 -07:00
float duty = mc_interface_get_duty_cycle_now ( ) ;
2015-02-19 12:20:07 -08:00
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 ) {
2019-04-18 11:00:26 -07:00
if ( send_current ) {
2020-07-03 06:16:46 -07:00
comm_can_set_current ( msg - > id , current_filtered ) ;
2019-04-18 11:00:26 -07:00
} else if ( send_duty ) {
comm_can_set_duty ( msg - > id , duty ) ;
}
2015-02-19 12:20:07 -08:00
}
}
}
2020-07-03 06:16:46 -07:00
//CTRL TYPE CURRENT
2014-09-21 10:29:26 -07:00
if ( current_mode ) {
2020-07-03 06:16:46 -07:00
if ( current_mode_brake ) { //If braking applied
mc_interface_set_brake_current ( fabsf ( current ) ) ;
2015-02-19 12:20:07 -08:00
// Send brake command to all ESCs seen recently on the CAN bus
2020-07-03 06:16:46 -07:00
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 ) ;
2015-02-19 12:20:07 -08:00
2020-07-03 06:16:46 -07:00
if ( msg - > id > = 0 & & UTILS_AGE_S ( msg - > rx_time ) < MAX_CAN_AGE ) {
comm_can_set_current_brake_rel ( msg - > id , fabsf ( servo_val ) ) ;
}
2015-02-19 12:20:07 -08:00
}
}
2014-09-21 10:29:26 -07:00
} else {
2015-02-19 12:20:07 -08:00
float current_out = current ;
bool is_reverse = false ;
2020-07-03 06:16:46 -07:00
static bool autoTCdisengaged = false ;
if ( current_out < 0.0 ) { // Not braking AND negative current = reverse engaged
2015-02-19 12:20:07 -08:00
is_reverse = true ;
current_out = - current_out ;
current = - current ;
rpm_local = - rpm_local ;
rpm_lowest = - rpm_lowest ;
2020-07-03 06:16:46 -07:00
rpm_highest = - rpm_highest ;
servo_val = - servo_val ;
2015-02-19 12:20:07 -08:00
}
2020-07-03 06:16:46 -07:00
// Send acceleration command to all ESCs seen recently on the CAN bus
2015-02-19 12:20:07 -08:00
if ( config . multi_esc ) {
2020-07-03 06:16:46 -07:00
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 ;
}
}
2015-02-19 12:20:07 -08:00
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 ) {
2020-07-03 06:16:46 -07:00
//Traction Control - Applied to slaves except if a fault has occured on the local VESC (undriven wheel may generate fake RPM)
if ( config . tc & & ! autoTCdisengaged ) {
2015-02-19 12:20:07 -08:00
float rpm_tmp = msg - > rpm ;
if ( is_reverse ) {
rpm_tmp = - rpm_tmp ;
}
float diff = rpm_tmp - rpm_lowest ;
2020-07-03 06:16:46 -07:00
servo_val = utils_map ( diff , 0.0 , config . tc_max_diff , servo_val , 0.0 ) ;
2015-02-19 12:20:07 -08:00
}
2020-07-03 06:16:46 -07:00
//Send motor drive command to slaves
2015-02-19 12:20:07 -08:00
if ( is_reverse ) {
2020-07-03 06:16:46 -07:00
comm_can_set_current_rel ( msg - > id , - servo_val ) ;
2015-02-19 12:20:07 -08:00
} else {
2020-07-03 06:16:46 -07:00
comm_can_set_current_rel ( msg - > id , servo_val ) ;
2015-02-19 12:20:07 -08:00
}
}
}
2020-07-03 06:16:46 -07:00
//Traction Control - Applying locally
2015-02-19 12:20:07 -08:00
if ( config . tc ) {
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 ;
}
}
}
2020-07-03 06:16:46 -07:00
//Drive local motor
2015-02-19 12:20:07 -08:00
if ( is_reverse ) {
2015-12-08 12:01:23 -08:00
mc_interface_set_current ( - current_out ) ;
2015-02-19 12:20:07 -08:00
} else {
2015-12-08 12:01:23 -08:00
mc_interface_set_current ( current_out ) ;
2015-02-19 12:20:07 -08:00
}
2014-09-21 10:29:26 -07:00
}
}
2014-04-18 15:09:46 -07:00
}
}
2015-07-31 14:26:50 -07:00
# endif