2015-05-04 14:25:43 -07:00
|
|
|
/*
|
2017-09-04 12:12:43 -07:00
|
|
|
Copyright 2016 - 2017 Benjamin Vedder benjamin@vedder.se
|
2015-05-04 14:25:43 -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
|
2015-05-04 14:25:43 -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,
|
2015-05-04 14:25:43 -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/>.
|
2017-09-04 12:12:43 -07:00
|
|
|
*/
|
2015-05-04 14:25:43 -07:00
|
|
|
|
|
|
|
#include "app.h"
|
|
|
|
|
|
|
|
#include "ch.h"
|
|
|
|
#include "hal.h"
|
|
|
|
#include "stm32f4xx_conf.h"
|
2015-12-08 12:01:23 -08:00
|
|
|
#include "mc_interface.h"
|
2015-05-04 14:25:43 -07:00
|
|
|
#include "timeout.h"
|
2022-03-16 10:40:51 -07:00
|
|
|
#include "utils_math.h"
|
|
|
|
#include "utils_sys.h"
|
2015-05-04 14:25:43 -07:00
|
|
|
#include "comm_can.h"
|
|
|
|
#include "hw.h"
|
|
|
|
#include <math.h>
|
|
|
|
|
|
|
|
// Settings
|
|
|
|
#define MAX_CAN_AGE 0.1
|
|
|
|
#define MIN_MS_WITHOUT_POWER 500
|
|
|
|
#define FILTER_SAMPLES 5
|
2015-08-23 09:26:05 -07:00
|
|
|
#define RPM_FILTER_SAMPLES 8
|
2021-09-08 15:16:58 -07:00
|
|
|
#define TC_DIFF_MAX_PASS 60 // TODO: move to app_conf
|
2015-05-04 14:25:43 -07:00
|
|
|
|
|
|
|
// Threads
|
2015-10-08 14:09:39 -07:00
|
|
|
static THD_FUNCTION(adc_thread, arg);
|
2022-01-16 12:57:12 -08:00
|
|
|
static THD_WORKING_AREA(adc_thread_wa, 512);
|
2015-05-04 14:25:43 -07:00
|
|
|
|
|
|
|
// Private variables
|
|
|
|
static volatile adc_config config;
|
2017-09-04 12:12:43 -07:00
|
|
|
static volatile float ms_without_power = 0.0;
|
2015-05-04 14:25:43 -07:00
|
|
|
static volatile float decoded_level = 0.0;
|
|
|
|
static volatile float read_voltage = 0.0;
|
2016-02-02 07:28:55 -08:00
|
|
|
static volatile float decoded_level2 = 0.0;
|
|
|
|
static volatile float read_voltage2 = 0.0;
|
2015-08-23 09:26:05 -07:00
|
|
|
static volatile bool use_rx_tx_as_buttons = false;
|
2016-11-04 07:18:34 -07:00
|
|
|
static volatile bool stop_now = true;
|
|
|
|
static volatile bool is_running = false;
|
2015-05-04 14:25:43 -07:00
|
|
|
|
|
|
|
void app_adc_configure(adc_config *conf) {
|
|
|
|
config = *conf;
|
|
|
|
ms_without_power = 0.0;
|
|
|
|
}
|
|
|
|
|
2015-08-23 09:26:05 -07:00
|
|
|
void app_adc_start(bool use_rx_tx) {
|
2022-05-03 13:53:10 -07:00
|
|
|
#ifdef HW_ADC_EXT_GPIO
|
|
|
|
palSetPadMode(HW_ADC_EXT_GPIO, HW_ADC_EXT_PIN, PAL_MODE_INPUT_ANALOG);
|
|
|
|
#endif
|
|
|
|
#ifdef HW_ADC_EXT2_GPIO
|
|
|
|
palSetPadMode(HW_ADC_EXT2_GPIO, HW_ADC_EXT2_PIN, PAL_MODE_INPUT_ANALOG);
|
|
|
|
#endif
|
|
|
|
|
2015-08-23 09:26:05 -07:00
|
|
|
use_rx_tx_as_buttons = use_rx_tx;
|
2016-11-04 07:18:34 -07:00
|
|
|
stop_now = false;
|
2015-05-04 14:25:43 -07:00
|
|
|
chThdCreateStatic(adc_thread_wa, sizeof(adc_thread_wa), NORMALPRIO, adc_thread, NULL);
|
|
|
|
}
|
|
|
|
|
2016-11-04 07:18:34 -07:00
|
|
|
void app_adc_stop(void) {
|
|
|
|
stop_now = true;
|
|
|
|
while (is_running) {
|
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-05-04 14:25:43 -07:00
|
|
|
float app_adc_get_decoded_level(void) {
|
|
|
|
return decoded_level;
|
|
|
|
}
|
|
|
|
|
|
|
|
float app_adc_get_voltage(void) {
|
|
|
|
return read_voltage;
|
|
|
|
}
|
|
|
|
|
2016-02-02 07:28:55 -08:00
|
|
|
float app_adc_get_decoded_level2(void) {
|
|
|
|
return decoded_level2;
|
|
|
|
}
|
|
|
|
|
|
|
|
float app_adc_get_voltage2(void) {
|
|
|
|
return read_voltage2;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2015-10-08 14:09:39 -07:00
|
|
|
static THD_FUNCTION(adc_thread, arg) {
|
2015-05-04 14:25:43 -07:00
|
|
|
(void)arg;
|
|
|
|
|
|
|
|
chRegSetThreadName("APP_ADC");
|
|
|
|
|
|
|
|
// Set servo pin as an input with pullup
|
2015-08-23 09:26:05 -07:00
|
|
|
if (use_rx_tx_as_buttons) {
|
|
|
|
palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_INPUT_PULLUP);
|
|
|
|
palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_INPUT_PULLUP);
|
|
|
|
} else {
|
|
|
|
palSetPadMode(HW_ICU_GPIO, HW_ICU_PIN, PAL_MODE_INPUT_PULLUP);
|
|
|
|
}
|
2015-05-04 14:25:43 -07:00
|
|
|
|
2016-11-04 07:18:34 -07:00
|
|
|
is_running = true;
|
|
|
|
|
2015-05-04 14:25:43 -07:00
|
|
|
for(;;) {
|
|
|
|
// Sleep for a time according to the specified rate
|
2015-10-08 14:09:39 -07:00
|
|
|
systime_t sleep_time = CH_CFG_ST_FREQUENCY / config.update_rate_hz;
|
2015-05-04 14:25:43 -07:00
|
|
|
|
|
|
|
// At least one tick should be slept to not block the other threads
|
|
|
|
if (sleep_time == 0) {
|
|
|
|
sleep_time = 1;
|
|
|
|
}
|
|
|
|
chThdSleep(sleep_time);
|
|
|
|
|
2016-11-04 07:18:34 -07:00
|
|
|
if (stop_now) {
|
|
|
|
is_running = false;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2015-08-23 09:26:05 -07:00
|
|
|
// For safe start when fault codes occur
|
2021-08-16 08:48:06 -07:00
|
|
|
if (mc_interface_get_fault() != FAULT_CODE_NONE && config.safe_start != SAFE_START_NO_FAULT) {
|
2015-08-23 09:26:05 -07:00
|
|
|
ms_without_power = 0;
|
|
|
|
}
|
|
|
|
|
2021-10-31 05:40:43 -07:00
|
|
|
// Read the external ADC pin voltage
|
|
|
|
float pwr = ADC_VOLTS(ADC_IND_EXT);
|
2015-05-04 14:25:43 -07:00
|
|
|
read_voltage = pwr;
|
|
|
|
|
2021-10-31 05:40:43 -07:00
|
|
|
// Optionally apply a filter
|
|
|
|
static float filter_val = 0.0;
|
|
|
|
UTILS_LP_MOVING_AVG_APPROX(filter_val, pwr, FILTER_SAMPLES);
|
2015-05-04 14:25:43 -07:00
|
|
|
|
2021-10-31 05:40:43 -07:00
|
|
|
if (config.use_filter) {
|
|
|
|
pwr = filter_val;
|
2015-05-04 14:25:43 -07:00
|
|
|
}
|
|
|
|
|
2017-09-04 12:12:43 -07:00
|
|
|
// Map the read voltage
|
|
|
|
switch (config.ctrl_type) {
|
|
|
|
case ADC_CTRL_TYPE_CURRENT_REV_CENTER:
|
2019-04-09 15:11:11 -07:00
|
|
|
case ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_CENTER:
|
2017-09-04 12:12:43 -07:00
|
|
|
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER:
|
|
|
|
case ADC_CTRL_TYPE_DUTY_REV_CENTER:
|
2017-10-27 12:29:12 -07:00
|
|
|
case ADC_CTRL_TYPE_PID_REV_CENTER:
|
2017-09-04 12:12:43 -07:00
|
|
|
// Mapping with respect to center voltage
|
|
|
|
if (pwr < config.voltage_center) {
|
|
|
|
pwr = utils_map(pwr, config.voltage_start,
|
|
|
|
config.voltage_center, 0.0, 0.5);
|
|
|
|
} else {
|
|
|
|
pwr = utils_map(pwr, config.voltage_center,
|
|
|
|
config.voltage_end, 0.5, 1.0);
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
|
|
|
default:
|
|
|
|
// Linear mapping between the start and end voltage
|
|
|
|
pwr = utils_map(pwr, config.voltage_start, config.voltage_end, 0.0, 1.0);
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Truncate the read voltage
|
2015-05-04 14:25:43 -07:00
|
|
|
utils_truncate_number(&pwr, 0.0, 1.0);
|
|
|
|
|
|
|
|
// Optionally invert the read voltage
|
|
|
|
if (config.voltage_inverted) {
|
|
|
|
pwr = 1.0 - pwr;
|
|
|
|
}
|
|
|
|
|
|
|
|
decoded_level = pwr;
|
|
|
|
|
2016-02-02 07:28:55 -08:00
|
|
|
// Read the external ADC pin and convert the value to a voltage.
|
2016-02-02 08:19:37 -08:00
|
|
|
#ifdef ADC_IND_EXT2
|
2021-10-31 05:40:43 -07:00
|
|
|
float brake = ADC_VOLTS(ADC_IND_EXT2);
|
2016-02-02 08:19:37 -08:00
|
|
|
#else
|
|
|
|
float brake = 0.0;
|
|
|
|
#endif
|
2016-02-02 07:28:55 -08:00
|
|
|
|
2021-04-19 07:33:57 -07:00
|
|
|
#ifdef HW_HAS_BRAKE_OVERRIDE
|
|
|
|
hw_brake_override(&brake);
|
|
|
|
#endif
|
2016-02-02 07:28:55 -08:00
|
|
|
read_voltage2 = brake;
|
|
|
|
|
2021-10-31 05:40:43 -07:00
|
|
|
// Optionally apply a filter
|
|
|
|
static float filter_val_2 = 0.0;
|
|
|
|
UTILS_LP_MOVING_AVG_APPROX(filter_val_2, brake, FILTER_SAMPLES);
|
2016-02-02 07:28:55 -08:00
|
|
|
|
2021-10-31 05:40:43 -07:00
|
|
|
if (config.use_filter) {
|
|
|
|
brake = filter_val_2;
|
2016-02-02 07:28:55 -08:00
|
|
|
}
|
|
|
|
|
|
|
|
// Map and truncate the read voltage
|
2016-11-04 07:18:34 -07:00
|
|
|
brake = utils_map(brake, config.voltage2_start, config.voltage2_end, 0.0, 1.0);
|
2016-02-02 07:28:55 -08:00
|
|
|
utils_truncate_number(&brake, 0.0, 1.0);
|
|
|
|
|
|
|
|
// Optionally invert the read voltage
|
2016-11-04 07:18:34 -07:00
|
|
|
if (config.voltage2_inverted) {
|
2016-02-02 07:28:55 -08:00
|
|
|
brake = 1.0 - brake;
|
|
|
|
}
|
|
|
|
|
|
|
|
decoded_level2 = brake;
|
|
|
|
|
2015-08-23 09:26:05 -07:00
|
|
|
// Read the button pins
|
|
|
|
bool cc_button = false;
|
|
|
|
bool rev_button = false;
|
|
|
|
if (use_rx_tx_as_buttons) {
|
|
|
|
cc_button = !palReadPad(HW_UART_TX_PORT, HW_UART_TX_PIN);
|
|
|
|
if (config.cc_button_inverted) {
|
|
|
|
cc_button = !cc_button;
|
|
|
|
}
|
|
|
|
rev_button = !palReadPad(HW_UART_RX_PORT, HW_UART_RX_PIN);
|
|
|
|
if (config.rev_button_inverted) {
|
|
|
|
rev_button = !rev_button;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
// When only one button input is available, use it differently depending on the control mode
|
|
|
|
if (config.ctrl_type == ADC_CTRL_TYPE_CURRENT_REV_BUTTON ||
|
2019-04-09 18:12:09 -07:00
|
|
|
config.ctrl_type == ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_CENTER ||
|
2015-08-23 09:26:05 -07:00
|
|
|
config.ctrl_type == ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON ||
|
2019-08-22 03:45:21 -07:00
|
|
|
config.ctrl_type == ADC_CTRL_TYPE_DUTY_REV_BUTTON ||
|
|
|
|
config.ctrl_type == ADC_CTRL_TYPE_PID_REV_BUTTON) {
|
2015-08-23 09:26:05 -07:00
|
|
|
rev_button = !palReadPad(HW_ICU_GPIO, HW_ICU_PIN);
|
|
|
|
if (config.rev_button_inverted) {
|
|
|
|
rev_button = !rev_button;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
cc_button = !palReadPad(HW_ICU_GPIO, HW_ICU_PIN);
|
|
|
|
if (config.cc_button_inverted) {
|
|
|
|
cc_button = !cc_button;
|
|
|
|
}
|
|
|
|
}
|
2015-05-04 14:25:43 -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;
|
|
|
|
}
|
|
|
|
|
2015-05-04 14:25:43 -07:00
|
|
|
switch (config.ctrl_type) {
|
|
|
|
case ADC_CTRL_TYPE_CURRENT_REV_CENTER:
|
2019-04-09 15:11:11 -07:00
|
|
|
case ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_CENTER:
|
2015-05-04 14:25:43 -07:00
|
|
|
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER:
|
|
|
|
case ADC_CTRL_TYPE_DUTY_REV_CENTER:
|
2017-10-27 12:29:12 -07:00
|
|
|
case ADC_CTRL_TYPE_PID_REV_CENTER:
|
2015-05-04 14:25:43 -07:00
|
|
|
// Scale the voltage and set 0 at the center
|
|
|
|
pwr *= 2.0;
|
|
|
|
pwr -= 1.0;
|
|
|
|
break;
|
|
|
|
|
2016-02-02 07:28:55 -08:00
|
|
|
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_ADC:
|
2017-09-04 12:12:43 -07:00
|
|
|
case ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_ADC:
|
|
|
|
pwr -= brake;
|
2016-02-02 07:28:55 -08:00
|
|
|
break;
|
|
|
|
|
2015-05-04 14:25:43 -07:00
|
|
|
case ADC_CTRL_TYPE_CURRENT_REV_BUTTON:
|
|
|
|
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON:
|
|
|
|
case ADC_CTRL_TYPE_DUTY_REV_BUTTON:
|
2017-10-27 12:29:12 -07:00
|
|
|
case ADC_CTRL_TYPE_PID_REV_BUTTON:
|
2015-05-04 14:25:43 -07:00
|
|
|
// Invert the voltage if the button is pressed
|
2015-08-23 09:26:05 -07:00
|
|
|
if (rev_button) {
|
2015-05-04 14:25:43 -07:00
|
|
|
pwr = -pwr;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
|
|
|
default:
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2015-08-23 09:26:05 -07:00
|
|
|
// Apply deadband
|
2015-05-04 14:25:43 -07:00
|
|
|
utils_deadband(&pwr, config.hyst, 1.0);
|
|
|
|
|
2017-09-04 12:12:43 -07:00
|
|
|
// Apply throttle curve
|
2017-09-06 12:13:28 -07:00
|
|
|
pwr = utils_throttle_curve(pwr, 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 pwr_ramp = 0.0;
|
2019-03-01 08:18:07 -08:00
|
|
|
float ramp_time = fabsf(pwr) > fabsf(pwr_ramp) ? config.ramp_time_pos : config.ramp_time_neg;
|
|
|
|
|
2017-09-04 12:12:43 -07:00
|
|
|
if (ramp_time > 0.01) {
|
|
|
|
const float ramp_step = (float)ST2MS(chVTTimeElapsedSinceX(last_time)) / (ramp_time * 1000.0);
|
|
|
|
utils_step_towards(&pwr_ramp, pwr, ramp_step);
|
|
|
|
last_time = chVTGetSystemTimeX();
|
|
|
|
pwr = pwr_ramp;
|
|
|
|
}
|
|
|
|
|
2018-01-24 00:39:06 -08:00
|
|
|
float current_rel = 0.0;
|
2015-05-04 14:25:43 -07:00
|
|
|
bool current_mode = false;
|
|
|
|
bool current_mode_brake = false;
|
2015-12-08 12:01:23 -08:00
|
|
|
const volatile mc_configuration *mcconf = mc_interface_get_configuration();
|
2017-09-04 12:12:43 -07:00
|
|
|
const float rpm_now = mc_interface_get_rpm();
|
2015-05-04 14:25:43 -07:00
|
|
|
bool send_duty = false;
|
|
|
|
|
|
|
|
// Use the filtered and mapped voltage for control according to the configuration.
|
|
|
|
switch (config.ctrl_type) {
|
|
|
|
case ADC_CTRL_TYPE_CURRENT:
|
|
|
|
case ADC_CTRL_TYPE_CURRENT_REV_CENTER:
|
|
|
|
case ADC_CTRL_TYPE_CURRENT_REV_BUTTON:
|
|
|
|
current_mode = true;
|
2021-11-26 09:06:07 -08:00
|
|
|
current_rel = pwr;
|
2015-05-04 14:25:43 -07:00
|
|
|
|
|
|
|
if (fabsf(pwr) < 0.001) {
|
2015-10-08 14:09:39 -07:00
|
|
|
ms_without_power += (1000.0 * (float)sleep_time) / (float)CH_CFG_ST_FREQUENCY;
|
2015-05-04 14:25:43 -07:00
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
2019-04-09 15:11:11 -07:00
|
|
|
case ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_CENTER:
|
2015-05-04 14:25:43 -07:00
|
|
|
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER:
|
|
|
|
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON:
|
2016-02-02 07:28:55 -08:00
|
|
|
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_ADC:
|
2017-09-04 12:12:43 -07:00
|
|
|
case ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_ADC:
|
2015-05-04 14:25:43 -07:00
|
|
|
current_mode = true;
|
|
|
|
if (pwr >= 0.0) {
|
2020-12-07 11:00:25 -08:00
|
|
|
// 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());
|
|
|
|
}
|
2018-01-24 00:39:06 -08:00
|
|
|
current_rel = pwr;
|
2015-05-04 14:25:43 -07:00
|
|
|
} else {
|
2018-01-24 00:39:06 -08:00
|
|
|
current_rel = fabsf(pwr);
|
2015-05-04 14:25:43 -07:00
|
|
|
current_mode_brake = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (pwr < 0.001) {
|
2015-10-08 14:09:39 -07:00
|
|
|
ms_without_power += (1000.0 * (float)sleep_time) / (float)CH_CFG_ST_FREQUENCY;
|
2015-05-04 14:25:43 -07:00
|
|
|
}
|
2017-09-04 12:12:43 -07:00
|
|
|
|
2019-04-09 15:11:11 -07:00
|
|
|
if ((config.ctrl_type == ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_ADC ||
|
|
|
|
config.ctrl_type == ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_CENTER) && rev_button) {
|
2018-01-24 00:39:06 -08:00
|
|
|
current_rel = -current_rel;
|
2017-09-04 12:12:43 -07:00
|
|
|
}
|
2015-05-04 14:25:43 -07:00
|
|
|
break;
|
|
|
|
|
|
|
|
case ADC_CTRL_TYPE_DUTY:
|
|
|
|
case ADC_CTRL_TYPE_DUTY_REV_CENTER:
|
|
|
|
case ADC_CTRL_TYPE_DUTY_REV_BUTTON:
|
|
|
|
if (fabsf(pwr) < 0.001) {
|
2015-10-08 14:09:39 -07:00
|
|
|
ms_without_power += (1000.0 * (float)sleep_time) / (float)CH_CFG_ST_FREQUENCY;
|
2015-05-04 14:25:43 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
if (!(ms_without_power < MIN_MS_WITHOUT_POWER && config.safe_start)) {
|
2016-02-09 03:23:58 -08:00
|
|
|
mc_interface_set_duty(utils_map(pwr, -1.0, 1.0, -mcconf->l_max_duty, mcconf->l_max_duty));
|
2015-05-04 14:25:43 -07:00
|
|
|
send_duty = true;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
2017-10-27 12:29:12 -07:00
|
|
|
case ADC_CTRL_TYPE_PID:
|
|
|
|
case ADC_CTRL_TYPE_PID_REV_CENTER:
|
|
|
|
case ADC_CTRL_TYPE_PID_REV_BUTTON:
|
|
|
|
if ((pwr >= 0.0 && rpm_now > 0.0) || (pwr < 0.0 && rpm_now < 0.0)) {
|
2018-01-24 00:39:06 -08:00
|
|
|
current_rel = pwr;
|
2017-10-27 12:29:12 -07:00
|
|
|
} else {
|
2018-01-24 00:39:06 -08:00
|
|
|
current_rel = pwr;
|
2017-10-27 12:29:12 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
if (!(ms_without_power < MIN_MS_WITHOUT_POWER && config.safe_start)) {
|
|
|
|
float speed = 0.0;
|
|
|
|
if (pwr >= 0.0) {
|
|
|
|
speed = pwr * mcconf->l_max_erpm;
|
|
|
|
} else {
|
|
|
|
speed = pwr * fabsf(mcconf->l_min_erpm);
|
|
|
|
}
|
|
|
|
|
|
|
|
mc_interface_set_pid_speed(speed);
|
|
|
|
send_duty = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (fabsf(pwr) < 0.001) {
|
|
|
|
ms_without_power += (1000.0 * (float)sleep_time) / (float)CH_CFG_ST_FREQUENCY;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
2015-05-04 14:25:43 -07:00
|
|
|
default:
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
2022-04-01 09:43:03 -07:00
|
|
|
bool range_ok = read_voltage >= config.voltage_min && read_voltage <= config.voltage_max;
|
|
|
|
|
2015-05-04 14:25:43 -07:00
|
|
|
// If safe start is enabled and the output has not been zero for long enough
|
2022-04-01 09:43:03 -07:00
|
|
|
if ((ms_without_power < MIN_MS_WITHOUT_POWER && config.safe_start) || !range_ok) {
|
2015-05-04 14:25:43 -07:00
|
|
|
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;
|
2015-12-08 12:01:23 -08:00
|
|
|
mc_interface_set_brake_current(timeout_get_brake_current());
|
2016-05-19 10:24:01 -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) {
|
|
|
|
comm_can_set_current_brake(msg->id, timeout_get_brake_current());
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-05-04 14:25:43 -07:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Reset timeout
|
|
|
|
timeout_reset();
|
|
|
|
|
2015-08-23 09:26:05 -07:00
|
|
|
// If c is pressed and no throttle is used, maintain the current speed with PID control
|
|
|
|
static bool was_pid = false;
|
|
|
|
|
|
|
|
// Filter RPM to avoid glitches
|
2021-10-31 05:40:43 -07:00
|
|
|
static float rpm_filtered = 0.0;
|
|
|
|
UTILS_LP_MOVING_AVG_APPROX(rpm_filtered, mc_interface_get_rpm(), RPM_FILTER_SAMPLES);
|
2015-08-23 09:26:05 -07:00
|
|
|
|
|
|
|
if (current_mode && cc_button && fabsf(pwr) < 0.001) {
|
|
|
|
static float pid_rpm = 0.0;
|
|
|
|
|
|
|
|
if (!was_pid) {
|
|
|
|
was_pid = true;
|
|
|
|
pid_rpm = rpm_filtered;
|
|
|
|
}
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
mc_interface_set_pid_speed(pid_rpm);
|
2015-08-23 09:26:05 -07:00
|
|
|
|
|
|
|
// Send the same duty cycle to the other controllers
|
|
|
|
if (config.multi_esc) {
|
2017-09-04 12:12:43 -07:00
|
|
|
float current = mc_interface_get_tot_current_directional_filtered();
|
2015-08-23 09:26:05 -07: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) {
|
2017-09-04 12:12:43 -07:00
|
|
|
comm_can_set_current(msg->id, current);
|
2015-08-23 09:26:05 -07:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
|
|
|
was_pid = false;
|
|
|
|
|
2015-05-04 14:25:43 -07:00
|
|
|
// Find lowest RPM (for traction control)
|
2015-12-08 12:01:23 -08:00
|
|
|
float rpm_local = mc_interface_get_rpm();
|
2015-05-04 14:25:43 -07:00
|
|
|
float rpm_lowest = rpm_local;
|
|
|
|
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) {
|
|
|
|
float rpm_tmp = msg->rpm;
|
|
|
|
|
|
|
|
if (fabsf(rpm_tmp) < fabsf(rpm_lowest)) {
|
|
|
|
rpm_lowest = rpm_tmp;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// Optionally send the duty cycles to the other ESCs seen on the CAN-bus
|
|
|
|
if (send_duty && config.multi_esc) {
|
2015-12-08 12:01:23 -08:00
|
|
|
float duty = mc_interface_get_duty_cycle_now();
|
2015-05-04 14:25:43 -07: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) {
|
|
|
|
comm_can_set_duty(msg->id, duty);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
if (current_mode) {
|
|
|
|
if (current_mode_brake) {
|
2018-01-24 00:39:06 -08:00
|
|
|
mc_interface_set_brake_current_rel(current_rel);
|
2015-05-04 14:25:43 -07:00
|
|
|
|
|
|
|
// Send brake command to all ESCs seen recently on the CAN bus
|
2019-07-01 08:17:49 -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-05-04 14:25:43 -07:00
|
|
|
|
2019-07-01 08:17:49 -07:00
|
|
|
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
|
|
|
|
comm_can_set_current_brake_rel(msg->id, current_rel);
|
|
|
|
}
|
2015-05-04 14:25:43 -07:00
|
|
|
}
|
|
|
|
}
|
|
|
|
} else {
|
2018-01-24 00:39:06 -08:00
|
|
|
float current_out = current_rel;
|
2015-05-04 14:25:43 -07:00
|
|
|
bool is_reverse = false;
|
|
|
|
if (current_out < 0.0) {
|
|
|
|
is_reverse = true;
|
|
|
|
current_out = -current_out;
|
2018-01-24 00:39:06 -08:00
|
|
|
current_rel = -current_rel;
|
2015-05-04 14:25:43 -07:00
|
|
|
rpm_local = -rpm_local;
|
|
|
|
rpm_lowest = -rpm_lowest;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Traction control
|
|
|
|
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) {
|
2021-03-27 03:18:47 -07:00
|
|
|
if (config.tc && config.tc_max_diff > 1.0) {
|
2015-05-04 14:25:43 -07:00
|
|
|
float rpm_tmp = msg->rpm;
|
|
|
|
if (is_reverse) {
|
|
|
|
rpm_tmp = -rpm_tmp;
|
|
|
|
}
|
|
|
|
|
|
|
|
float diff = rpm_tmp - rpm_lowest;
|
2021-09-08 15:16:58 -07:00
|
|
|
if (diff < TC_DIFF_MAX_PASS) diff = 0;
|
|
|
|
if (diff > config.tc_max_diff) diff = config.tc_max_diff;
|
2018-01-24 00:39:06 -08:00
|
|
|
current_out = utils_map(diff, 0.0, config.tc_max_diff, current_rel, 0.0);
|
2015-05-04 14:25:43 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
if (is_reverse) {
|
2018-01-24 00:39:06 -08:00
|
|
|
comm_can_set_current_rel(msg->id, -current_out);
|
2015-05-04 14:25:43 -07:00
|
|
|
} else {
|
2018-01-24 00:39:06 -08:00
|
|
|
comm_can_set_current_rel(msg->id, current_out);
|
2015-05-04 14:25:43 -07:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
if (config.tc) {
|
|
|
|
float diff = rpm_local - rpm_lowest;
|
2021-09-08 15:16:58 -07:00
|
|
|
if (diff < TC_DIFF_MAX_PASS) diff = 0;
|
|
|
|
if (diff > config.tc_max_diff) diff = config.tc_max_diff;
|
2018-01-24 00:39:06 -08:00
|
|
|
current_out = utils_map(diff, 0.0, config.tc_max_diff, current_rel, 0.0);
|
2015-05-04 14:25:43 -07:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
if (is_reverse) {
|
2018-01-24 00:39:06 -08:00
|
|
|
mc_interface_set_current_rel(-current_out);
|
2015-05-04 14:25:43 -07:00
|
|
|
} else {
|
2018-01-24 00:39:06 -08:00
|
|
|
mc_interface_set_current_rel(current_out);
|
2015-05-04 14:25:43 -07:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|