mirror of https://github.com/rusefi/bldc.git
PPM dec fix, current sampling updates, ADC cruise control, commutation fix, higher switching frequency, refactoring
This commit is contained in:
Benjamin Vedder
parent
383208bdbd
commit
58d892935d
2
Makefile
2
Makefile
|
|
@ -6,7 +6,7 @@
|
|||
# Compiler options here.
|
||||
ifeq ($(USE_OPT),)
|
||||
USE_OPT = -O2 -ggdb -fomit-frame-pointer -falign-functions=16 -std=gnu99
|
||||
USE_OPT += -DBOARD_OTG_NOVBUSSENS
|
||||
USE_OPT += -DBOARD_OTG_NOVBUSSENS $(build_args)
|
||||
endif
|
||||
|
||||
# C specific options here (added to USE_OPT).
|
||||
|
|
|
|||
|
|
@ -40,7 +40,7 @@ void app_init(app_configuration *conf) {
|
|||
break;
|
||||
|
||||
case APP_ADC:
|
||||
app_adc_start();
|
||||
app_adc_start(true);
|
||||
break;
|
||||
|
||||
case APP_UART:
|
||||
|
|
@ -56,7 +56,7 @@ void app_init(app_configuration *conf) {
|
|||
|
||||
case APP_ADC_UART:
|
||||
hw_stop_i2c();
|
||||
app_adc_start();
|
||||
app_adc_start(false);
|
||||
app_uartcomm_start();
|
||||
break;
|
||||
|
||||
|
|
|
|||
|
|
@ -35,7 +35,7 @@ void app_set_configuration(app_configuration *conf);
|
|||
// Standard apps
|
||||
void app_ppm_start(void);
|
||||
void app_ppm_configure(ppm_config *conf);
|
||||
void app_adc_start(void);
|
||||
void app_adc_start(bool use_rx_tx);
|
||||
void app_adc_configure(adc_config *conf);
|
||||
float app_adc_get_decoded_level(void);
|
||||
float app_adc_get_voltage(void);
|
||||
|
|
|
|||
|
|
@ -38,6 +38,7 @@
|
|||
#define MAX_CAN_AGE 0.1
|
||||
#define MIN_MS_WITHOUT_POWER 500
|
||||
#define FILTER_SAMPLES 5
|
||||
#define RPM_FILTER_SAMPLES 8
|
||||
|
||||
// Threads
|
||||
static msg_t adc_thread(void *arg);
|
||||
|
|
@ -48,13 +49,15 @@ static volatile adc_config config;
|
|||
static volatile float ms_without_power = 0;
|
||||
static volatile float decoded_level = 0.0;
|
||||
static volatile float read_voltage = 0.0;
|
||||
static volatile bool use_rx_tx_as_buttons = false;
|
||||
|
||||
void app_adc_configure(adc_config *conf) {
|
||||
config = *conf;
|
||||
ms_without_power = 0.0;
|
||||
}
|
||||
|
||||
void app_adc_start(void) {
|
||||
void app_adc_start(bool use_rx_tx) {
|
||||
use_rx_tx_as_buttons = use_rx_tx;
|
||||
chThdCreateStatic(adc_thread_wa, sizeof(adc_thread_wa), NORMALPRIO, adc_thread, NULL);
|
||||
}
|
||||
|
||||
|
|
@ -72,7 +75,12 @@ static msg_t adc_thread(void *arg) {
|
|||
chRegSetThreadName("APP_ADC");
|
||||
|
||||
// Set servo pin as an input with pullup
|
||||
palSetPadMode(HW_ICU_GPIO, HW_ICU_PIN, PAL_MODE_INPUT_PULLUP);
|
||||
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);
|
||||
}
|
||||
|
||||
for(;;) {
|
||||
// Sleep for a time according to the specified rate
|
||||
|
|
@ -84,6 +92,11 @@ static msg_t adc_thread(void *arg) {
|
|||
}
|
||||
chThdSleep(sleep_time);
|
||||
|
||||
// For safe start when fault codes occur
|
||||
if (mcpwm_get_fault() != FAULT_CODE_NONE) {
|
||||
ms_without_power = 0;
|
||||
}
|
||||
|
||||
// Read the external ADC pin and convert the value to a voltage.
|
||||
float pwr = (float)ADC_Value[ADC_IND_EXT];
|
||||
pwr /= 4095;
|
||||
|
|
@ -119,10 +132,33 @@ static msg_t adc_thread(void *arg) {
|
|||
|
||||
decoded_level = pwr;
|
||||
|
||||
// Read the servo pin and optionally invert it.
|
||||
bool button_val = !palReadPad(HW_ICU_GPIO, HW_ICU_PIN);
|
||||
if (config.button_inverted) {
|
||||
button_val = !button_val;
|
||||
// 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 ||
|
||||
config.ctrl_type == ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON ||
|
||||
config.ctrl_type == ADC_CTRL_TYPE_DUTY_REV_BUTTON) {
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
switch (config.ctrl_type) {
|
||||
|
|
@ -138,7 +174,7 @@ static msg_t adc_thread(void *arg) {
|
|||
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON:
|
||||
case ADC_CTRL_TYPE_DUTY_REV_BUTTON:
|
||||
// Invert the voltage if the button is pressed
|
||||
if (button_val) {
|
||||
if (rev_button) {
|
||||
pwr = -pwr;
|
||||
}
|
||||
break;
|
||||
|
|
@ -147,10 +183,10 @@ static msg_t adc_thread(void *arg) {
|
|||
break;
|
||||
}
|
||||
|
||||
// Apply a deadband
|
||||
// Apply deadband
|
||||
utils_deadband(&pwr, config.hyst, 1.0);
|
||||
|
||||
float current = 0;
|
||||
float current = 0.0;
|
||||
bool current_mode = false;
|
||||
bool current_mode_brake = false;
|
||||
const volatile mc_configuration *mcconf = mcpwm_get_configuration();
|
||||
|
|
@ -219,6 +255,51 @@ static msg_t adc_thread(void *arg) {
|
|||
// Reset timeout
|
||||
timeout_reset();
|
||||
|
||||
// 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
|
||||
static float filter_buffer[RPM_FILTER_SAMPLES];
|
||||
static int filter_ptr = 0;
|
||||
filter_buffer[filter_ptr++] = mcpwm_get_rpm();
|
||||
if (filter_ptr >= RPM_FILTER_SAMPLES) {
|
||||
filter_ptr = 0;
|
||||
}
|
||||
|
||||
float rpm_filtered = 0.0;
|
||||
for (int i = 0;i < RPM_FILTER_SAMPLES;i++) {
|
||||
rpm_filtered += filter_buffer[i];
|
||||
}
|
||||
rpm_filtered /= RPM_FILTER_SAMPLES;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
mcpwm_set_pid_speed(pid_rpm);
|
||||
|
||||
// Send the same duty cycle to the other controllers
|
||||
if (config.multi_esc) {
|
||||
float duty = mcpwm_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) {
|
||||
comm_can_set_duty(msg->id, duty);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
continue;
|
||||
}
|
||||
|
||||
was_pid = false;
|
||||
|
||||
// Find lowest RPM (for traction control)
|
||||
float rpm_local = mcpwm_get_rpm();
|
||||
float rpm_lowest = rpm_local;
|
||||
|
|
|
|||
|
|
@ -235,13 +235,12 @@ static msg_t output_thread(void *arg) {
|
|||
// Filter RPM to avoid glitches
|
||||
static float filter_buffer[RPM_FILTER_SAMPLES];
|
||||
static int filter_ptr = 0;
|
||||
float rpm_filtered = mcpwm_get_rpm();
|
||||
filter_buffer[filter_ptr++] = rpm_filtered;
|
||||
filter_buffer[filter_ptr++] = mcpwm_get_rpm();
|
||||
if (filter_ptr >= RPM_FILTER_SAMPLES) {
|
||||
filter_ptr = 0;
|
||||
}
|
||||
|
||||
rpm_filtered = 0.0;
|
||||
float rpm_filtered = 0.0;
|
||||
for (int i = 0;i < RPM_FILTER_SAMPLES;i++) {
|
||||
rpm_filtered += filter_buffer[i];
|
||||
}
|
||||
|
|
|
|||
|
|
@ -390,7 +390,8 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
|
|||
appconf.app_adc_conf.voltage_end = (float)buffer_get_int32(data, &ind) / 1000.0;
|
||||
appconf.app_adc_conf.use_filter = data[ind++];
|
||||
appconf.app_adc_conf.safe_start = data[ind++];
|
||||
appconf.app_adc_conf.button_inverted = data[ind++];
|
||||
appconf.app_adc_conf.cc_button_inverted = data[ind++];
|
||||
appconf.app_adc_conf.rev_button_inverted = data[ind++];
|
||||
appconf.app_adc_conf.voltage_inverted = data[ind++];
|
||||
appconf.app_adc_conf.rpm_lim_start = (float)buffer_get_int32(data, &ind) / 1000.0;
|
||||
appconf.app_adc_conf.rpm_lim_end = (float)buffer_get_int32(data, &ind) / 1000.0;
|
||||
|
|
@ -452,7 +453,8 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
|
|||
buffer_append_int32(send_buffer, (int32_t)(appconf.app_adc_conf.voltage_end * 1000.0), &ind);
|
||||
send_buffer[ind++] = appconf.app_adc_conf.use_filter;
|
||||
send_buffer[ind++] = appconf.app_adc_conf.safe_start;
|
||||
send_buffer[ind++] = appconf.app_adc_conf.button_inverted;
|
||||
send_buffer[ind++] = appconf.app_adc_conf.cc_button_inverted;
|
||||
send_buffer[ind++] = appconf.app_adc_conf.rev_button_inverted;
|
||||
send_buffer[ind++] = appconf.app_adc_conf.voltage_inverted;
|
||||
buffer_append_int32(send_buffer, (int32_t)(appconf.app_adc_conf.rpm_lim_start * 1000.0), &ind);
|
||||
buffer_append_int32(send_buffer, (int32_t)(appconf.app_adc_conf.rpm_lim_end * 1000.0), &ind);
|
||||
|
|
|
|||
|
|
@ -139,9 +139,9 @@ void conf_general_init(void) {
|
|||
VirtAddVarTab[ind++] = EEPROM_BASE_APPCONF + i;
|
||||
}
|
||||
|
||||
FLASH_Unlock();
|
||||
FLASH_ClearFlag(FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR |
|
||||
FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR);
|
||||
FLASH_Unlock();
|
||||
EE_Init();
|
||||
}
|
||||
|
||||
|
|
@ -198,7 +198,8 @@ void conf_general_read_app_configuration(app_configuration *conf) {
|
|||
conf->app_adc_conf.voltage_end = 3.0;
|
||||
conf->app_adc_conf.use_filter = true;
|
||||
conf->app_adc_conf.safe_start = true;
|
||||
conf->app_adc_conf.button_inverted = false;
|
||||
conf->app_adc_conf.cc_button_inverted = false;
|
||||
conf->app_adc_conf.rev_button_inverted = false;
|
||||
conf->app_adc_conf.voltage_inverted = false;
|
||||
conf->app_adc_conf.rpm_lim_start = 150000;
|
||||
conf->app_adc_conf.rpm_lim_end = 200000;
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@
|
|||
|
||||
// Firmware version
|
||||
#define FW_VERSION_MAJOR 1
|
||||
#define FW_VERSION_MINOR 10
|
||||
#define FW_VERSION_MINOR 11
|
||||
|
||||
#include "datatypes.h"
|
||||
|
||||
|
|
@ -38,19 +38,17 @@
|
|||
#define SYSTEM_CORE_CLOCK 168000000
|
||||
|
||||
// Component parameters to override
|
||||
//#define V_REG 3.3
|
||||
#define VIN_R1 39000.0
|
||||
//#define VIN_R1 33000.0
|
||||
//#define VIN_R2 2200.0
|
||||
//#define CURRENT_AMP_GAIN 10.0
|
||||
//#define CURRENT_SHUNT_RES 0.001
|
||||
|
||||
// Correction factor for computations that depend on the old resistor division factor
|
||||
#define VDIV_CORR ((VIN_R2 / (VIN_R2 + VIN_R1)) / (2.2 / (2.2 + 33.0)))
|
||||
//#define CURRENT_SHUNT_RES 0.0005
|
||||
//#define WS2811_ENABLE 1
|
||||
//#define CURR1_DOUBLE_SAMPLE 0
|
||||
//#define CURR2_DOUBLE_SAMPLE 0
|
||||
|
||||
/*
|
||||
* Select only one hardware version
|
||||
*/
|
||||
//#define HW_VERSION_BW
|
||||
//#define HW_VERSION_40
|
||||
//#define HW_VERSION_45
|
||||
#define HW_VERSION_46 // Also for 4.7
|
||||
|
|
@ -94,7 +92,9 @@
|
|||
* Output WS2811 signal on the HALL1 pin. Notice that hall sensors can't be used
|
||||
* at the same time.
|
||||
*/
|
||||
#ifndef WS2811_ENABLE
|
||||
#define WS2811_ENABLE 0
|
||||
#endif
|
||||
#define WS2811_CLK_HZ 800000
|
||||
#define WS2811_LED_NUM 14
|
||||
#define WS2811_USE_CH2 1 // 0: CH1 (PB6) 1: CH2 (PB7)
|
||||
|
|
@ -102,12 +102,21 @@
|
|||
/*
|
||||
* Servo output driver
|
||||
*/
|
||||
#ifndef SERVO_OUT_ENABLE
|
||||
#define SERVO_OUT_ENABLE 0 // Enable servo output
|
||||
#endif
|
||||
#define SERVO_OUT_SIMPLE 1 // Use simple HW-based driver (recommended)
|
||||
#define SERVO_OUT_PULSE_MIN_US 1000 // Minimum pulse length in microseconds
|
||||
#define SERVO_OUT_PULSE_MAX_US 2000 // Maximum pulse length in microseconds
|
||||
#define SERVO_OUT_RATE_HZ 50 // Update rate in Hz
|
||||
|
||||
// Correction factor for computations that depend on the old resistor division factor
|
||||
#define VDIV_CORR ((VIN_R2 / (VIN_R2 + VIN_R1)) / (2.2 / (2.2 + 33.0)))
|
||||
|
||||
// Actual voltage on 3.3V net based on internal reference
|
||||
//#define V_REG (1.21 / ((float)ADC_Value[ADC_IND_VREFINT] / 4095.0))
|
||||
#define V_REG 3.3
|
||||
|
||||
// Functions
|
||||
void conf_general_init(void);
|
||||
void conf_general_read_app_configuration(app_configuration *conf);
|
||||
|
|
|
|||
|
|
@ -208,7 +208,8 @@ typedef struct {
|
|||
float voltage_end;
|
||||
bool use_filter;
|
||||
bool safe_start;
|
||||
bool button_inverted;
|
||||
bool cc_button_inverted;
|
||||
bool rev_button_inverted;
|
||||
bool voltage_inverted;
|
||||
float rpm_lim_start;
|
||||
float rpm_lim_end;
|
||||
|
|
@ -316,8 +317,10 @@ typedef struct {
|
|||
float duty;
|
||||
float rpm;
|
||||
int tacho;
|
||||
int tim_pwm_cnt;
|
||||
int tim_samp_cnt;
|
||||
int cycles_running;
|
||||
int tim_val_samp;
|
||||
int tim_current_samp;
|
||||
int tim_top;
|
||||
int comm_step;
|
||||
float temperature;
|
||||
} fault_data;
|
||||
|
|
|
|||
|
|
@ -114,27 +114,29 @@ void hw_init_gpio(void) {
|
|||
}
|
||||
|
||||
void hw_setup_adc_channels(void) {
|
||||
// ADC1 regular channels 0, 5, 10, 13
|
||||
// ADC1 regular channels
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_5, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_Vrefint, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_13, 4, ADC_SampleTime_15Cycles);
|
||||
|
||||
// ADC2 regular channels 1, 6, 11, 15
|
||||
// ADC2 regular channels
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_1, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_6, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_11, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_15, 4, ADC_SampleTime_15Cycles);
|
||||
|
||||
// ADC3 regular channels 2, 3, 12, 3
|
||||
// ADC3 regular channels
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_2, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_3, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_12, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_3, 4, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_10, 4, ADC_SampleTime_15Cycles);
|
||||
|
||||
// Injected channels
|
||||
ADC_InjectedChannelConfig(ADC1, ADC_Channel_6, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_InjectedChannelConfig(ADC2, ADC_Channel_5, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_InjectedChannelConfig(ADC1, ADC_Channel_5, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_InjectedChannelConfig(ADC2, ADC_Channel_6, 2, ADC_SampleTime_15Cycles);
|
||||
}
|
||||
|
||||
void hw_setup_servo_outputs(void) {
|
||||
|
|
|
|||
|
|
@ -46,12 +46,12 @@
|
|||
* 3: IN5 CURR2
|
||||
* 4: IN6 CURR1
|
||||
* 5: IN3 NC
|
||||
* 6: IN10 TEMP_MOTOR
|
||||
* 6: VREFINT
|
||||
* 7: IN11 NC
|
||||
* 8: IN12 AN_IN
|
||||
* 9: IN13 NC
|
||||
* 10: IN15 ADC_EXT
|
||||
* 11: IN3 NC
|
||||
* 11: IN10 TEMP_MOTOR
|
||||
*/
|
||||
|
||||
#define HW_ADC_CHANNELS 12
|
||||
|
|
@ -65,6 +65,7 @@
|
|||
#define ADC_IND_CURR2 3
|
||||
#define ADC_IND_VIN_SENS 8
|
||||
#define ADC_IND_EXT 10
|
||||
#define ADC_IND_VREFINT 6
|
||||
|
||||
// ADC macros and settings
|
||||
|
||||
|
|
@ -95,6 +96,15 @@
|
|||
#define NTC_RES(adc_val) (0.0)
|
||||
#define NTC_TEMP(adc_ind) (32.0)
|
||||
|
||||
// Double samples in beginning and end for positive current measurement.
|
||||
// Useful when the shunt sense traces have noise that causes offset.
|
||||
#ifndef CURR1_DOUBLE_SAMPLE
|
||||
#define CURR1_DOUBLE_SAMPLE 1
|
||||
#endif
|
||||
#ifndef CURR2_DOUBLE_SAMPLE
|
||||
#define CURR2_DOUBLE_SAMPLE 0
|
||||
#endif
|
||||
|
||||
// Number of servo outputs
|
||||
#define HW_SERVO_NUM 2
|
||||
|
||||
|
|
|
|||
|
|
@ -120,27 +120,29 @@ void hw_init_gpio(void) {
|
|||
}
|
||||
|
||||
void hw_setup_adc_channels(void) {
|
||||
// ADC1 regular channels 0, 5, 10, 13
|
||||
// ADC1 regular channels
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_5, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_Vrefint, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_13, 4, ADC_SampleTime_15Cycles);
|
||||
|
||||
// ADC2 regular channels 1, 6, 11, 15
|
||||
// ADC2 regular channels
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_1, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_6, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_11, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_15, 4, ADC_SampleTime_15Cycles);
|
||||
|
||||
// ADC3 regular channels 2, 3, 12, 3
|
||||
// ADC3 regular channels
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_2, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_3, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_12, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_3, 4, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_10, 4, ADC_SampleTime_15Cycles);
|
||||
|
||||
// Injected channels
|
||||
ADC_InjectedChannelConfig(ADC1, ADC_Channel_6, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_InjectedChannelConfig(ADC2, ADC_Channel_5, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_InjectedChannelConfig(ADC1, ADC_Channel_5, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_InjectedChannelConfig(ADC2, ADC_Channel_6, 2, ADC_SampleTime_15Cycles);
|
||||
|
||||
// Setup i2c temperature sensor here
|
||||
chThdCreateStatic(temp_thread_wa, sizeof(temp_thread_wa), NORMALPRIO, temp_thread, NULL);
|
||||
|
|
|
|||
|
|
@ -46,12 +46,12 @@
|
|||
* 3: IN5 CURR2
|
||||
* 4: IN6 CURR1
|
||||
* 5: IN3 NC
|
||||
* 6: IN10 TEMP_MOTOR
|
||||
* 6: VREFINT
|
||||
* 7: IN11 NC
|
||||
* 8: IN12 AN_IN
|
||||
* 9: IN13 NC
|
||||
* 10: IN15 ADC_EXT
|
||||
* 11: IN3 NC
|
||||
* 11: IN10 TEMP_MOTOR
|
||||
*/
|
||||
|
||||
#define HW_ADC_CHANNELS 12
|
||||
|
|
@ -65,6 +65,7 @@
|
|||
#define ADC_IND_CURR2 3
|
||||
#define ADC_IND_VIN_SENS 8
|
||||
#define ADC_IND_EXT 10
|
||||
#define ADC_IND_VREFINT 6
|
||||
|
||||
// ADC macros and settings
|
||||
|
||||
|
|
@ -95,6 +96,15 @@
|
|||
#define NTC_RES(adc_val) (0.0)
|
||||
#define NTC_TEMP(adc_ind) hw45_get_temp()
|
||||
|
||||
// Double samples in beginning and end for positive current measurement.
|
||||
// Useful when the shunt sense traces have noise that causes offset.
|
||||
#ifndef CURR1_DOUBLE_SAMPLE
|
||||
#define CURR1_DOUBLE_SAMPLE 1
|
||||
#endif
|
||||
#ifndef CURR2_DOUBLE_SAMPLE
|
||||
#define CURR2_DOUBLE_SAMPLE 0
|
||||
#endif
|
||||
|
||||
// Number of servo outputs
|
||||
#define HW_SERVO_NUM 2
|
||||
|
||||
|
|
|
|||
|
|
@ -114,27 +114,29 @@ void hw_init_gpio(void) {
|
|||
}
|
||||
|
||||
void hw_setup_adc_channels(void) {
|
||||
// ADC1 regular channels 0, 5, 10, 4
|
||||
// ADC1 regular channels
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_5, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_Vrefint, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_4, 4, ADC_SampleTime_15Cycles);
|
||||
|
||||
// ADC2 regular channels 1, 6, 11, 15
|
||||
// ADC2 regular channels
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_1, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_6, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_11, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_15, 4, ADC_SampleTime_15Cycles);
|
||||
|
||||
// ADC3 regular channels 2, 3, 12, 3
|
||||
// ADC3 regular channels
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_2, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_3, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_12, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_3, 4, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_10, 4, ADC_SampleTime_15Cycles);
|
||||
|
||||
// Injected channels
|
||||
ADC_InjectedChannelConfig(ADC1, ADC_Channel_6, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_InjectedChannelConfig(ADC1, ADC_Channel_5, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_InjectedChannelConfig(ADC2, ADC_Channel_5, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_InjectedChannelConfig(ADC2, ADC_Channel_6, 2, ADC_SampleTime_15Cycles);
|
||||
}
|
||||
|
||||
void hw_setup_servo_outputs(void) {
|
||||
|
|
|
|||
|
|
@ -46,12 +46,12 @@
|
|||
* 3: IN5 CURR2
|
||||
* 4: IN6 CURR1
|
||||
* 5: IN3 NC
|
||||
* 6: IN10 TEMP_MOTOR
|
||||
* 6: Vrefint
|
||||
* 7: IN11 NC
|
||||
* 8: IN12 AN_IN
|
||||
* 9: IN4 TEMP_MOSFET
|
||||
* 10: IN15 ADC_EXT
|
||||
* 11: IN3 NC
|
||||
* 11: IN10 TEMP_MOTOR
|
||||
*/
|
||||
|
||||
#define HW_ADC_CHANNELS 12
|
||||
|
|
@ -72,15 +72,16 @@
|
|||
#define ADC_IND_TEMP_MOS5 9
|
||||
#define ADC_IND_TEMP_MOS6 9
|
||||
#define ADC_IND_TEMP_PCB 9
|
||||
#define ADC_IND_VREFINT 6
|
||||
|
||||
// ADC macros and settings
|
||||
|
||||
// Component parameters (can be overridden)
|
||||
#ifndef V_REG
|
||||
#define V_REG 3.3
|
||||
//#define V_REG 3.3
|
||||
#endif
|
||||
#ifndef VIN_R1
|
||||
#define VIN_R1 33000.0
|
||||
#define VIN_R1 39000.0
|
||||
#endif
|
||||
#ifndef VIN_R2
|
||||
#define VIN_R2 2200.0
|
||||
|
|
@ -103,6 +104,15 @@
|
|||
#define NTC_RES(adc_val) ((4095.0 * 10000.0) / adc_val - 10000.0)
|
||||
#define NTC_TEMP(adc_ind) (1.0 / ((logf(NTC_RES(ADC_Value[adc_ind]) / 10000.0) / 3434.0) + (1.0 / 298.15)) - 273.15)
|
||||
|
||||
// Double samples in beginning and end for positive current measurement.
|
||||
// Useful when the shunt sense traces have noise that causes offset.
|
||||
#ifndef CURR1_DOUBLE_SAMPLE
|
||||
#define CURR1_DOUBLE_SAMPLE 1
|
||||
#endif
|
||||
#ifndef CURR2_DOUBLE_SAMPLE
|
||||
#define CURR2_DOUBLE_SAMPLE 0
|
||||
#endif
|
||||
|
||||
// Number of servo outputs
|
||||
#define HW_SERVO_NUM 2
|
||||
|
||||
|
|
|
|||
250
hwconf/hw_bw.c
250
hwconf/hw_bw.c
|
|
@ -1,250 +0,0 @@
|
|||
/*
|
||||
Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se
|
||||
|
||||
This program 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.
|
||||
|
||||
This program 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/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* hw_bw.c
|
||||
*
|
||||
* Created on: 14 apr 2014
|
||||
* Author: benjamin
|
||||
*/
|
||||
|
||||
#include "hw.h"
|
||||
#ifdef HW_VERSION_BW
|
||||
|
||||
#include "ch.h"
|
||||
#include "hal.h"
|
||||
#include "stm32f4xx_conf.h"
|
||||
#include "servo.h"
|
||||
|
||||
// Variables
|
||||
static volatile bool i2c_running = false;
|
||||
|
||||
// I2C configuration
|
||||
static const I2CConfig i2cfg = {
|
||||
OPMODE_I2C,
|
||||
100000,
|
||||
STD_DUTY_CYCLE
|
||||
};
|
||||
|
||||
void hw_init_gpio(void) {
|
||||
// GPIO clock enable
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
|
||||
|
||||
// LEDs
|
||||
palSetPadMode(GPIOC, 4,
|
||||
PAL_MODE_OUTPUT_PUSHPULL |
|
||||
PAL_STM32_OSPEED_HIGHEST);
|
||||
palSetPadMode(GPIOA, 7,
|
||||
PAL_MODE_OUTPUT_PUSHPULL |
|
||||
PAL_STM32_OSPEED_HIGHEST);
|
||||
|
||||
// GPIOC (ENABLE_GATE)
|
||||
palSetPadMode(GPIOC, 10,
|
||||
PAL_MODE_OUTPUT_PUSHPULL |
|
||||
PAL_STM32_OSPEED_HIGHEST);
|
||||
DISABLE_GATE();
|
||||
|
||||
// GPIOB (DCCAL)
|
||||
palSetPadMode(GPIOB, 12,
|
||||
PAL_MODE_OUTPUT_PUSHPULL |
|
||||
PAL_STM32_OSPEED_HIGHEST);
|
||||
|
||||
// GPIOA Configuration: Channel 1 to 3 as alternate function push-pull
|
||||
palSetPadMode(GPIOA, 8, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
|
||||
PAL_STM32_OSPEED_HIGHEST |
|
||||
PAL_STM32_PUDR_FLOATING);
|
||||
palSetPadMode(GPIOA, 9, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
|
||||
PAL_STM32_OSPEED_HIGHEST |
|
||||
PAL_STM32_PUDR_FLOATING);
|
||||
palSetPadMode(GPIOA, 10, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
|
||||
PAL_STM32_OSPEED_HIGHEST |
|
||||
PAL_STM32_PUDR_FLOATING);
|
||||
|
||||
palSetPadMode(GPIOB, 13, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
|
||||
PAL_STM32_OSPEED_HIGHEST |
|
||||
PAL_STM32_PUDR_FLOATING);
|
||||
palSetPadMode(GPIOB, 14, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
|
||||
PAL_STM32_OSPEED_HIGHEST |
|
||||
PAL_STM32_PUDR_FLOATING);
|
||||
palSetPadMode(GPIOB, 15, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
|
||||
PAL_STM32_OSPEED_HIGHEST |
|
||||
PAL_STM32_PUDR_FLOATING);
|
||||
|
||||
// Hall sensors
|
||||
palSetPadMode(HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1, PAL_MODE_INPUT_PULLUP);
|
||||
palSetPadMode(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, PAL_MODE_INPUT_PULLUP);
|
||||
palSetPadMode(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3, PAL_MODE_INPUT_PULLUP);
|
||||
|
||||
// Fault pin
|
||||
palSetPadMode(GPIOC, 12, PAL_MODE_INPUT_PULLUP);
|
||||
|
||||
// ADC Pins
|
||||
palSetPadMode(GPIOA, 0, PAL_MODE_INPUT_ANALOG);
|
||||
palSetPadMode(GPIOA, 1, PAL_MODE_INPUT_ANALOG);
|
||||
palSetPadMode(GPIOA, 2, PAL_MODE_INPUT_ANALOG);
|
||||
palSetPadMode(GPIOA, 3, PAL_MODE_INPUT_ANALOG);
|
||||
palSetPadMode(GPIOA, 4, PAL_MODE_INPUT_ANALOG);
|
||||
palSetPadMode(GPIOA, 5, PAL_MODE_INPUT_ANALOG);
|
||||
palSetPadMode(GPIOA, 6, PAL_MODE_INPUT_ANALOG);
|
||||
|
||||
palSetPadMode(GPIOB, 0, PAL_MODE_INPUT_ANALOG);
|
||||
palSetPadMode(GPIOB, 1, PAL_MODE_INPUT_ANALOG);
|
||||
|
||||
palSetPadMode(GPIOC, 0, PAL_MODE_INPUT_ANALOG);
|
||||
palSetPadMode(GPIOC, 1, PAL_MODE_INPUT_ANALOG);
|
||||
palSetPadMode(GPIOC, 2, PAL_MODE_INPUT_ANALOG);
|
||||
palSetPadMode(GPIOC, 3, PAL_MODE_INPUT_ANALOG);
|
||||
palSetPadMode(GPIOC, 5, PAL_MODE_INPUT_ANALOG);
|
||||
}
|
||||
|
||||
void hw_setup_adc_channels(void) {
|
||||
// ADC1 regular channels 0, 5, 10, 13
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_5, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_13, 4, ADC_SampleTime_15Cycles);
|
||||
|
||||
// ADC2 regular channels 1, 6, 11, 15
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_1, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_6, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_11, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_15, 4, ADC_SampleTime_15Cycles);
|
||||
|
||||
// ADC3 regular channels 2, 3, 12, 3
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_2, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_3, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_12, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_3, 4, ADC_SampleTime_15Cycles);
|
||||
|
||||
// Injected channels
|
||||
ADC_InjectedChannelConfig(ADC1, ADC_Channel_6, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_InjectedChannelConfig(ADC2, ADC_Channel_5, 1, ADC_SampleTime_15Cycles);
|
||||
}
|
||||
|
||||
void hw_setup_servo_outputs(void) {
|
||||
// Set up GPIO ports
|
||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
|
||||
|
||||
// Set up servo structures
|
||||
servos[0].gpio = GPIOB;
|
||||
servos[0].pin = 5;
|
||||
servos[0].offset = 0;
|
||||
servos[0].pos = 128;
|
||||
|
||||
servos[1].gpio = GPIOB;
|
||||
servos[1].pin = 4;
|
||||
servos[1].offset = 0;
|
||||
servos[1].pos = 0;
|
||||
}
|
||||
|
||||
void hw_start_i2c(void) {
|
||||
i2cAcquireBus(&HW_I2C_DEV);
|
||||
|
||||
if (!i2c_running) {
|
||||
palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN,
|
||||
PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) |
|
||||
PAL_STM32_OTYPE_OPENDRAIN |
|
||||
PAL_STM32_OSPEED_MID1 |
|
||||
PAL_STM32_PUDR_PULLUP);
|
||||
palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
|
||||
PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) |
|
||||
PAL_STM32_OTYPE_OPENDRAIN |
|
||||
PAL_STM32_OSPEED_MID1 |
|
||||
PAL_STM32_PUDR_PULLUP);
|
||||
|
||||
i2cStart(&HW_I2C_DEV, &i2cfg);
|
||||
i2c_running = true;
|
||||
}
|
||||
|
||||
i2cReleaseBus(&HW_I2C_DEV);
|
||||
}
|
||||
|
||||
void hw_stop_i2c(void) {
|
||||
i2cAcquireBus(&HW_I2C_DEV);
|
||||
|
||||
if (i2c_running) {
|
||||
palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN, PAL_MODE_INPUT);
|
||||
palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN, PAL_MODE_INPUT);
|
||||
|
||||
i2cStop(&HW_I2C_DEV);
|
||||
i2c_running = false;
|
||||
|
||||
}
|
||||
|
||||
i2cReleaseBus(&HW_I2C_DEV);
|
||||
}
|
||||
|
||||
/**
|
||||
* Try to restore the i2c bus
|
||||
*/
|
||||
void hw_try_restore_i2c(void) {
|
||||
if (i2c_running) {
|
||||
i2cAcquireBus(&HW_I2C_DEV);
|
||||
|
||||
palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN,
|
||||
PAL_STM32_OTYPE_OPENDRAIN |
|
||||
PAL_STM32_OSPEED_MID1 |
|
||||
PAL_STM32_PUDR_PULLUP);
|
||||
|
||||
palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
|
||||
PAL_STM32_OTYPE_OPENDRAIN |
|
||||
PAL_STM32_OSPEED_MID1 |
|
||||
PAL_STM32_PUDR_PULLUP);
|
||||
|
||||
palSetPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
|
||||
palSetPad(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN);
|
||||
|
||||
chThdSleep(1);
|
||||
|
||||
for(int i = 0;i < 16;i++) {
|
||||
palClearPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
|
||||
chThdSleep(1);
|
||||
palSetPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
|
||||
chThdSleep(1);
|
||||
}
|
||||
|
||||
// Generate start then stop condition
|
||||
palClearPad(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN);
|
||||
chThdSleep(1);
|
||||
palClearPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
|
||||
chThdSleep(1);
|
||||
palSetPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
|
||||
chThdSleep(1);
|
||||
palSetPad(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN);
|
||||
|
||||
palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN,
|
||||
PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) |
|
||||
PAL_STM32_OTYPE_OPENDRAIN |
|
||||
PAL_STM32_OSPEED_MID1 |
|
||||
PAL_STM32_PUDR_PULLUP);
|
||||
|
||||
palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
|
||||
PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) |
|
||||
PAL_STM32_OTYPE_OPENDRAIN |
|
||||
PAL_STM32_OSPEED_MID1 |
|
||||
PAL_STM32_PUDR_PULLUP);
|
||||
|
||||
HW_I2C_DEV.state = I2C_STOP;
|
||||
i2cStart(&HW_I2C_DEV, &i2cfg);
|
||||
|
||||
i2cReleaseBus(&HW_I2C_DEV);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
160
hwconf/hw_bw.h
160
hwconf/hw_bw.h
|
|
@ -1,160 +0,0 @@
|
|||
/*
|
||||
Copyright 2012-2014 Benjamin Vedder benjamin@vedder.se
|
||||
|
||||
This program 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.
|
||||
|
||||
This program 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/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* hw_40.h
|
||||
*
|
||||
* Created on: 14 apr 2014
|
||||
* Author: benjamin
|
||||
*/
|
||||
|
||||
#ifndef HW_40_H_
|
||||
#define HW_40_H_
|
||||
|
||||
// Macros
|
||||
#define ENABLE_GATE() palSetPad(GPIOC, 10)
|
||||
#define DISABLE_GATE() palClearPad(GPIOC, 10)
|
||||
#define DCCAL_ON() palSetPad(GPIOB, 12)
|
||||
#define DCCAL_OFF() palClearPad(GPIOB, 12)
|
||||
#define IS_DRV_FAULT() (!palReadPad(GPIOC, 12))
|
||||
|
||||
#define LED_GREEN_ON() palSetPad(GPIOC, 4)
|
||||
#define LED_GREEN_OFF() palClearPad(GPIOC, 4)
|
||||
#define LED_RED_ON() palSetPad(GPIOA, 7)
|
||||
#define LED_RED_OFF() palClearPad(GPIOA, 7)
|
||||
|
||||
/*
|
||||
* ADC Vector
|
||||
*
|
||||
* 0: IN0 SENS3
|
||||
* 1: IN1 SENS2
|
||||
* 2: IN2 SENS1
|
||||
* 3: IN5 CURR2
|
||||
* 4: IN6 CURR1
|
||||
* 5: IN3 NC
|
||||
* 6: IN10 TEMP_MOTOR
|
||||
* 7: IN11 NC
|
||||
* 8: IN12 AN_IN
|
||||
* 9: IN13 NC
|
||||
* 10: IN15 ADC_EXT
|
||||
* 11: IN3 NC
|
||||
*/
|
||||
|
||||
#define HW_ADC_CHANNELS 12
|
||||
#define HW_ADC_NBR_CONV 4
|
||||
|
||||
// ADC Indexes
|
||||
#define ADC_IND_SENS1 2
|
||||
#define ADC_IND_SENS2 1
|
||||
#define ADC_IND_SENS3 0
|
||||
#define ADC_IND_CURR1 4
|
||||
#define ADC_IND_CURR2 3
|
||||
#define ADC_IND_VIN_SENS 8
|
||||
#define ADC_IND_EXT 10
|
||||
|
||||
// ADC macros and settings
|
||||
|
||||
// Component parameters (can be overridden)
|
||||
#ifndef V_REG
|
||||
#define V_REG 3.3
|
||||
#endif
|
||||
#ifndef VIN_R1
|
||||
#define VIN_R1 33000.0
|
||||
#endif
|
||||
#ifndef VIN_R2
|
||||
#define VIN_R2 2200.0
|
||||
#endif
|
||||
#ifndef CURRENT_AMP_GAIN
|
||||
#define CURRENT_AMP_GAIN 10.0
|
||||
#endif
|
||||
#ifndef CURRENT_SHUNT_RES
|
||||
#define CURRENT_SHUNT_RES 0.010
|
||||
#endif
|
||||
|
||||
// Input voltage
|
||||
#define GET_INPUT_VOLTAGE() ((V_REG / 4095.0) * (float)ADC_Value[ADC_IND_VIN_SENS] * ((VIN_R1 + VIN_R2) / VIN_R2))
|
||||
|
||||
// Voltage on ADC channel
|
||||
#define ADC_VOLTS(ch) ((float)ADC_Value[ch] / 4095.0 * V_REG)
|
||||
|
||||
// NTC Termistors
|
||||
#define NTC_RES(adc_val) (0.0)
|
||||
#define NTC_TEMP(adc_ind) (32.0)
|
||||
|
||||
// Number of servo outputs
|
||||
#define HW_SERVO_NUM 2
|
||||
|
||||
// UART Peripheral
|
||||
#define HW_UART_DEV UARTD6
|
||||
#define HW_UART_GPIO_AF GPIO_AF_USART6
|
||||
#define HW_UART_TX_PORT GPIOC
|
||||
#define HW_UART_TX_PIN 6
|
||||
#define HW_UART_RX_PORT GPIOC
|
||||
#define HW_UART_RX_PIN 7
|
||||
|
||||
// ICU Peripheral for servo decoding
|
||||
#define HW_ICU_CHANNEL ICU_CHANNEL_2
|
||||
#define HW_ICU_GPIO_AF GPIO_AF_TIM3
|
||||
#define HW_ICU_GPIO GPIOB
|
||||
#define HW_ICU_PIN 5
|
||||
|
||||
// I2C Peripheral
|
||||
#define HW_I2C_DEV I2CD2
|
||||
#define HW_I2C_GPIO_AF GPIO_AF_I2C2
|
||||
#define HW_I2C_SCL_PORT GPIOB
|
||||
#define HW_I2C_SCL_PIN 10
|
||||
#define HW_I2C_SDA_PORT GPIOB
|
||||
#define HW_I2C_SDA_PIN 11
|
||||
|
||||
// Hall/encoder pins
|
||||
#define HW_HALL_ENC_GPIO1 GPIOB
|
||||
#define HW_HALL_ENC_PIN1 6
|
||||
#define HW_HALL_ENC_GPIO2 GPIOB
|
||||
#define HW_HALL_ENC_PIN2 7
|
||||
#define HW_HALL_ENC_GPIO3 GPIOB
|
||||
#define HW_HALL_ENC_PIN3 8
|
||||
#define HW_ENC_TIM TIM4
|
||||
#define HW_ENC_TIM_AF GPIO_AF_TIM4
|
||||
#define HW_ENC_TIM_CLK_EN() RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE)
|
||||
#define HW_ENC_EXTI_PORTSRC EXTI_PortSourceGPIOB
|
||||
#define HW_ENC_EXTI_PINSRC EXTI_PinSource8
|
||||
#define HW_ENC_EXTI_CH EXTI9_5_IRQn
|
||||
#define HW_ENC_EXTI_LINE EXTI_Line8
|
||||
#define HW_ENC_EXTI_ISR_VEC EXTI9_5_IRQHandler
|
||||
|
||||
// NRF pins
|
||||
#define NRF_PORT_CSN HW_ICU_GPIO
|
||||
#define NRF_PIN_CSN HW_ICU_PIN
|
||||
#define NRF_PORT_SCK GPIOC
|
||||
#define NRF_PIN_SCK 5
|
||||
#define NRF_PORT_MOSI HW_I2C_SDA_PORT
|
||||
#define NRF_PIN_MOSI HW_I2C_SDA_PIN
|
||||
#define NRF_PORT_MISO HW_I2C_SCL_PORT
|
||||
#define NRF_PIN_MISO HW_I2C_SCL_PIN
|
||||
|
||||
// Measurement macros
|
||||
#define ADC_V_L1 ADC_Value[ADC_IND_SENS1]
|
||||
#define ADC_V_L2 ADC_Value[ADC_IND_SENS2]
|
||||
#define ADC_V_L3 ADC_Value[ADC_IND_SENS3]
|
||||
#define ADC_V_ZERO (ADC_Value[ADC_IND_VIN_SENS] / 2)
|
||||
|
||||
// Macros
|
||||
#define READ_HALL1() palReadPad(HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1)
|
||||
#define READ_HALL2() palReadPad(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2)
|
||||
#define READ_HALL3() palReadPad(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3)
|
||||
|
||||
#endif /* HW_40_H_ */
|
||||
|
|
@ -116,21 +116,21 @@ void hw_init_gpio(void) {
|
|||
}
|
||||
|
||||
void hw_setup_adc_channels(void) {
|
||||
// ADC1 regular channels 0, 5, 4, 7, 14
|
||||
// ADC1 regular channels
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_5, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_4, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_7, 4, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_Vrefint, 4, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_14, 5, ADC_SampleTime_15Cycles);
|
||||
|
||||
// ADC2 regular channels 1, 6, 9, 15, 8
|
||||
// ADC2 regular channels
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_1, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_6, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_9, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_15, 4, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_8, 5, ADC_SampleTime_15Cycles);
|
||||
|
||||
// ADC3 regular channels 2, 10, 12, 11, 13
|
||||
// ADC3 regular channels
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_2, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_10, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_12, 3, ADC_SampleTime_15Cycles);
|
||||
|
|
@ -140,6 +140,8 @@ void hw_setup_adc_channels(void) {
|
|||
// Injected channels
|
||||
ADC_InjectedChannelConfig(ADC1, ADC_Channel_5, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_InjectedChannelConfig(ADC2, ADC_Channel_6, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_InjectedChannelConfig(ADC1, ADC_Channel_6, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_InjectedChannelConfig(ADC2, ADC_Channel_5, 2, ADC_SampleTime_15Cycles);
|
||||
}
|
||||
|
||||
void hw_setup_servo_outputs(void) {
|
||||
|
|
|
|||
|
|
@ -49,7 +49,7 @@
|
|||
* 6: IN4 TEMP_PCB
|
||||
* 7: IN9 TEMP_MOS_5
|
||||
* 8: IN12 TEMP_MOS_2
|
||||
* 9: IN7 TEMP_MOS_6
|
||||
* 9: Vrefint
|
||||
* 10: IN15 ADC_EXT
|
||||
* 11: IN11 TEMP_MOS_1
|
||||
* 12: IN14 AN_IN
|
||||
|
|
@ -73,8 +73,9 @@
|
|||
#define ADC_IND_TEMP_MOS3 14
|
||||
#define ADC_IND_TEMP_MOS4 13
|
||||
#define ADC_IND_TEMP_MOS5 7
|
||||
#define ADC_IND_TEMP_MOS6 9
|
||||
#define ADC_IND_TEMP_MOS6 7 // TODO! Same as MOS5
|
||||
#define ADC_IND_TEMP_PCB 6
|
||||
#define ADC_IND_VREFINT 9
|
||||
|
||||
// ADC macros and settings
|
||||
|
||||
|
|
@ -105,6 +106,15 @@
|
|||
// Voltage on ADC channel
|
||||
#define ADC_VOLTS(ch) ((float)ADC_Value[ch] / 4096.0 * V_REG)
|
||||
|
||||
// Double samples in beginning and end for positive current measurement.
|
||||
// Useful when the shunt sense traces have noise that causes offset.
|
||||
#ifndef CURR1_DOUBLE_SAMPLE
|
||||
#define CURR1_DOUBLE_SAMPLE 0
|
||||
#endif
|
||||
#ifndef CURR2_DOUBLE_SAMPLE
|
||||
#define CURR2_DOUBLE_SAMPLE 0
|
||||
#endif
|
||||
|
||||
// Number of servo outputs
|
||||
#define HW_SERVO_NUM 2
|
||||
|
||||
|
|
|
|||
|
|
@ -114,27 +114,29 @@ void hw_init_gpio(void) {
|
|||
}
|
||||
|
||||
void hw_setup_adc_channels(void) {
|
||||
// ADC1 regular channels 0, 5, 10, 13
|
||||
// ADC1 regular channels
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_5, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_Vrefint, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_13, 4, ADC_SampleTime_15Cycles);
|
||||
|
||||
// ADC2 regular channels 1, 6, 11, 15
|
||||
// ADC2 regular channels
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_1, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_6, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_11, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_15, 4, ADC_SampleTime_15Cycles);
|
||||
|
||||
// ADC3 regular channels 2, 3, 12, 3
|
||||
// ADC3 regular channels
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_2, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_3, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_12, 3, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_3, 4, ADC_SampleTime_15Cycles);
|
||||
ADC_RegularChannelConfig(ADC3, ADC_Channel_10, 4, ADC_SampleTime_15Cycles);
|
||||
|
||||
// Injected channels
|
||||
ADC_InjectedChannelConfig(ADC1, ADC_Channel_5, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_InjectedChannelConfig(ADC2, ADC_Channel_6, 1, ADC_SampleTime_15Cycles);
|
||||
ADC_InjectedChannelConfig(ADC1, ADC_Channel_6, 2, ADC_SampleTime_15Cycles);
|
||||
ADC_InjectedChannelConfig(ADC2, ADC_Channel_5, 2, ADC_SampleTime_15Cycles);
|
||||
}
|
||||
|
||||
void hw_setup_servo_outputs(void) {
|
||||
|
|
|
|||
|
|
@ -46,7 +46,7 @@
|
|||
* 3: IN5 CURR1
|
||||
* 4: IN6 CURR2
|
||||
* 5: IN3 NC
|
||||
* 6: IN10 TEMP_MOTOR
|
||||
* 6: Vrefint
|
||||
* 7: IN11 NC
|
||||
* 8: IN12 AN_IN
|
||||
* 9: IN13 NC
|
||||
|
|
@ -65,6 +65,7 @@
|
|||
#define ADC_IND_CURR2 4
|
||||
#define ADC_IND_VIN_SENS 8
|
||||
#define ADC_IND_EXT 10
|
||||
#define ADC_IND_VREFINT 6
|
||||
|
||||
// ADC macros and settings
|
||||
|
||||
|
|
@ -95,6 +96,15 @@
|
|||
#define NTC_RES(adc_val) (0.0)
|
||||
#define NTC_TEMP(adc_ind) (32.0)
|
||||
|
||||
// Double samples in beginning and end for positive current measurement.
|
||||
// Useful when the shunt sense traces have noise that causes offset.
|
||||
#ifndef CURR1_DOUBLE_SAMPLE
|
||||
#define CURR1_DOUBLE_SAMPLE 0
|
||||
#endif
|
||||
#ifndef CURR2_DOUBLE_SAMPLE
|
||||
#define CURR2_DOUBLE_SAMPLE 0
|
||||
#endif
|
||||
|
||||
// Number of servo outputs
|
||||
#define HW_SERVO_NUM 2
|
||||
|
||||
|
|
|
|||
|
|
@ -1,7 +1,6 @@
|
|||
HWSRC = hwconf/hw_40.c \
|
||||
hwconf/hw_45.c \
|
||||
hwconf/hw_r2.c \
|
||||
hwconf/hw_bw.c \
|
||||
hwconf/hw_46.c \
|
||||
hwconf/hw_victor_r1a.c
|
||||
|
||||
|
|
|
|||
2
main.c
2
main.c
|
|
@ -246,7 +246,7 @@ void main_dma_adc_handler(void) {
|
|||
|
||||
vzero_samples[sample_now] = mcpwm_vzero;
|
||||
|
||||
curr_fir_samples[sample_now] = (int16_t)(mcpwm_get_tot_current_filtered() * 100.0);
|
||||
curr_fir_samples[sample_now] = (int16_t)(mcpwm_get_tot_current() * 100.0);
|
||||
f_sw_samples[sample_now] = (int16_t)(mcpwm_get_switching_frequency_now() / 10.0);
|
||||
|
||||
status_samples[sample_now] = mcpwm_get_comm_step() | (mcpwm_read_hall_phase() << 3);
|
||||
|
|
|
|||
263
mcpwm.c
263
mcpwm.c
|
|
@ -90,6 +90,7 @@ static volatile float cycle_integrator_sum;
|
|||
static volatile float cycle_integrator_iterations;
|
||||
static volatile mc_configuration conf;
|
||||
static volatile float pwm_cycles_sum;
|
||||
static volatile float pwm_cycles;
|
||||
static volatile float last_pwm_cycles_sum;
|
||||
static volatile float last_pwm_cycles_sums[6];
|
||||
static volatile float amp_seconds;
|
||||
|
|
@ -212,6 +213,7 @@ void mcpwm_init(mc_configuration *configuration) {
|
|||
cycle_integrator_sum = 0.0;
|
||||
cycle_integrator_iterations = 0.0;
|
||||
pwm_cycles_sum = 0.0;
|
||||
pwm_cycles = 0.0;
|
||||
last_pwm_cycles_sum = 0.0;
|
||||
memset((float*)last_pwm_cycles_sums, 0, sizeof(last_pwm_cycles_sums));
|
||||
amp_seconds = 0.0;
|
||||
|
|
@ -349,7 +351,8 @@ void mcpwm_init(mc_configuration *configuration) {
|
|||
ADC_Init(ADC2, &ADC_InitStructure);
|
||||
ADC_Init(ADC3, &ADC_InitStructure);
|
||||
|
||||
hw_setup_adc_channels();
|
||||
// Enable Vrefint channel
|
||||
ADC_TempSensorVrefintCmd(ENABLE);
|
||||
|
||||
// Enable DMA request after last transfer (Multi-ADC mode)
|
||||
ADC_MultiModeDMARequestAfterLastTransferCmd(ENABLE);
|
||||
|
|
@ -359,8 +362,10 @@ void mcpwm_init(mc_configuration *configuration) {
|
|||
ADC_ExternalTrigInjectedConvConfig(ADC2, ADC_ExternalTrigInjecConv_T8_CC2);
|
||||
ADC_ExternalTrigInjectedConvEdgeConfig(ADC1, ADC_ExternalTrigInjecConvEdge_Falling);
|
||||
ADC_ExternalTrigInjectedConvEdgeConfig(ADC2, ADC_ExternalTrigInjecConvEdge_Falling);
|
||||
ADC_InjectedSequencerLengthConfig(ADC1, 1);
|
||||
ADC_InjectedSequencerLengthConfig(ADC2, 1);
|
||||
ADC_InjectedSequencerLengthConfig(ADC1, 2);
|
||||
ADC_InjectedSequencerLengthConfig(ADC2, 2);
|
||||
|
||||
hw_setup_adc_channels();
|
||||
|
||||
// Interrupt
|
||||
ADC_ITConfig(ADC1, ADC_IT_JEOC, ENABLE);
|
||||
|
|
@ -830,7 +835,7 @@ float mcpwm_get_kv_filtered(void) {
|
|||
* The motor current.
|
||||
*/
|
||||
float mcpwm_get_tot_current(void) {
|
||||
return last_current_sample * (V_REG / 4095.0) / (CURRENT_SHUNT_RES * CURRENT_AMP_GAIN);
|
||||
return last_current_sample;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -842,7 +847,7 @@ float mcpwm_get_tot_current(void) {
|
|||
* The filtered motor current.
|
||||
*/
|
||||
float mcpwm_get_tot_current_filtered(void) {
|
||||
return last_current_sample_filtered * (V_REG / 4095.0) / (CURRENT_SHUNT_RES * CURRENT_AMP_GAIN);
|
||||
return last_current_sample_filtered;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -1059,8 +1064,9 @@ static void fault_stop(mc_fault_code fault) {
|
|||
// Sent to terminal fault logger so that all faults and their conditions
|
||||
// can be printed for debugging.
|
||||
chSysLock();
|
||||
volatile int t1_cnt = TIM1->CNT;
|
||||
volatile int t8_cnt = TIM8->CNT;
|
||||
volatile int val_samp = TIM8->CCR1;
|
||||
volatile int current_samp = TIM1->CCR4;
|
||||
volatile int tim_top = TIM1->ARR;
|
||||
chSysUnlock();
|
||||
|
||||
fault_data fdata;
|
||||
|
|
@ -1071,8 +1077,10 @@ static void fault_stop(mc_fault_code fault) {
|
|||
fdata.duty = dutycycle_now;
|
||||
fdata.rpm = mcpwm_get_rpm();
|
||||
fdata.tacho = mcpwm_get_tachometer_value(false);
|
||||
fdata.tim_pwm_cnt = t1_cnt;
|
||||
fdata.tim_samp_cnt = t8_cnt;
|
||||
fdata.cycles_running = cycles_running;
|
||||
fdata.tim_val_samp = val_samp;
|
||||
fdata.tim_current_samp = current_samp;
|
||||
fdata.tim_top = tim_top;
|
||||
fdata.comm_step = comm_step;
|
||||
fdata.temperature = NTC_TEMP(ADC_IND_TEMP_MOS1);
|
||||
terminal_add_fault_data(&fdata);
|
||||
|
|
@ -1183,19 +1191,32 @@ static void set_duty_cycle_ll(float dutyCycle) {
|
|||
}
|
||||
|
||||
if (dutyCycle < conf.l_min_duty) {
|
||||
float max_erpm_fbrake;
|
||||
if (control_mode == CONTROL_MODE_CURRENT || control_mode == CONTROL_MODE_CURRENT_BRAKE) {
|
||||
max_erpm_fbrake = conf.l_max_erpm_fbrake_cc;
|
||||
} else {
|
||||
max_erpm_fbrake = conf.l_max_erpm_fbrake;
|
||||
}
|
||||
|
||||
switch (state) {
|
||||
case MC_STATE_RUNNING:
|
||||
full_brake_ll();
|
||||
// TODO!!!
|
||||
if (fabsf(rpm_now) > max_erpm_fbrake) {
|
||||
dutyCycle = conf.l_min_duty;
|
||||
} else {
|
||||
full_brake_ll();
|
||||
return;
|
||||
}
|
||||
break;
|
||||
|
||||
case MC_STATE_DETECTING:
|
||||
stop_pwm_ll();
|
||||
return;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
return;
|
||||
}
|
||||
return;
|
||||
} else if (dutyCycle > conf.l_max_duty) {
|
||||
dutyCycle = conf.l_max_duty;
|
||||
}
|
||||
|
|
@ -1434,12 +1455,7 @@ static msg_t timer_thread(void *arg) {
|
|||
float max_s;
|
||||
|
||||
for(;;) {
|
||||
if (state != MC_STATE_OFF) {
|
||||
tachometer_for_direction = 0;
|
||||
}
|
||||
|
||||
switch (state) {
|
||||
case MC_STATE_OFF:
|
||||
if (state == MC_STATE_OFF) {
|
||||
// Track the motor back-emf and follow it with dutycycle_now. Also track
|
||||
// the direction of the motor.
|
||||
amp = filter_run_fir_iteration((float*)amp_fir_samples,
|
||||
|
|
@ -1510,19 +1526,8 @@ static msg_t timer_thread(void *arg) {
|
|||
dutycycle_now = -amp / (float)ADC_Value[ADC_IND_VIN_SENS];
|
||||
}
|
||||
utils_truncate_number((float*)&dutycycle_now, -conf.l_max_duty, conf.l_max_duty);
|
||||
break;
|
||||
|
||||
case MC_STATE_DETECTING:
|
||||
break;
|
||||
|
||||
case MC_STATE_RUNNING:
|
||||
break;
|
||||
|
||||
case MC_STATE_FULL_BRAKE:
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
} else {
|
||||
tachometer_for_direction = 0;
|
||||
}
|
||||
|
||||
// Fill KV filter vector at 100Hz
|
||||
|
|
@ -1569,22 +1574,68 @@ void mcpwm_adc_inj_int_handler(void) {
|
|||
int curr0 = ADC_GetInjectedConversionValue(ADC1, ADC_InjectedChannel_1);
|
||||
int curr1 = ADC_GetInjectedConversionValue(ADC2, ADC_InjectedChannel_1);
|
||||
|
||||
int curr0_2 = ADC_GetInjectedConversionValue(ADC2, ADC_InjectedChannel_2);
|
||||
int curr1_2 = ADC_GetInjectedConversionValue(ADC1, ADC_InjectedChannel_2);
|
||||
|
||||
float curr0_currsamp = curr0;
|
||||
float curr1_currsamp = curr1;
|
||||
|
||||
if (curr_samp_volt == 1) {
|
||||
curr0 = ADC_Value[ADC_IND_CURR1];
|
||||
} else if (curr_samp_volt == 2) {
|
||||
curr1 = ADC_Value[ADC_IND_CURR2];
|
||||
} else if (curr_samp_volt == 3) {
|
||||
curr0 = ADC_Value[ADC_IND_CURR1];
|
||||
curr1 = ADC_Value[ADC_IND_CURR2];
|
||||
}
|
||||
|
||||
// DCCal every other cycle
|
||||
// static bool sample_ofs = true;
|
||||
// if (sample_ofs) {
|
||||
// sample_ofs = false;
|
||||
// curr0_offset = curr0;
|
||||
// curr1_offset = curr1;
|
||||
// DCCAL_OFF();
|
||||
// return;
|
||||
// } else {
|
||||
// sample_ofs = true;
|
||||
// DCCAL_ON();
|
||||
// }
|
||||
|
||||
curr0_sum += curr0;
|
||||
curr1_sum += curr1;
|
||||
curr_start_samples++;
|
||||
|
||||
ADC_curr_norm_value[0] = curr0 - curr0_offset;
|
||||
ADC_curr_norm_value[1] = curr1 - curr1_offset;
|
||||
curr0_currsamp -= curr0_offset;
|
||||
curr1_currsamp -= curr1_offset;
|
||||
curr0 -= curr0_offset;
|
||||
curr1 -= curr1_offset;
|
||||
curr0_2 -= curr0_offset;
|
||||
curr1_2 -= curr1_offset;
|
||||
|
||||
#if CURR1_DOUBLE_SAMPLE || CURR2_DOUBLE_SAMPLE
|
||||
if (conf.pwm_mode != PWM_MODE_BIPOLAR && conf.motor_type == MOTOR_TYPE_BLDC) {
|
||||
if (direction) {
|
||||
if (CURR1_DOUBLE_SAMPLE && comm_step == 3) {
|
||||
curr0 = (curr0 + curr0_2) / 2.0;
|
||||
} else if (CURR2_DOUBLE_SAMPLE && comm_step == 4) {
|
||||
curr1 = (curr1 + curr1_2) / 2.0;
|
||||
}
|
||||
} else {
|
||||
if (CURR1_DOUBLE_SAMPLE && comm_step == 2) {
|
||||
curr0 = (curr0 + curr0_2) / 2.0;
|
||||
} else if (CURR2_DOUBLE_SAMPLE && comm_step == 1) {
|
||||
curr1 = (curr1 + curr1_2) / 2.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
ADC_curr_norm_value[0] = curr0;
|
||||
ADC_curr_norm_value[1] = curr1;
|
||||
ADC_curr_norm_value[2] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[1]);
|
||||
|
||||
float curr_tot_sample = 0;
|
||||
|
||||
if (conf.motor_type == MOTOR_TYPE_DC) {
|
||||
if (direction) {
|
||||
curr_tot_sample = -(float)(ADC_Value[ADC_IND_CURR2] - curr1_offset);
|
||||
|
|
@ -1620,43 +1671,41 @@ void mcpwm_adc_inj_int_handler(void) {
|
|||
float c2 = (float)ADC_curr_norm_value[2];
|
||||
curr_tot_sample = sqrtf((c0*c0 + c1*c1 + c2*c2) / 1.5);
|
||||
} else {
|
||||
switch (comm_step) {
|
||||
case 1:
|
||||
case 6:
|
||||
if (direction) {
|
||||
if (comm_step == 1) {
|
||||
curr_tot_sample = -(float)ADC_curr_norm_value[1];
|
||||
} else {
|
||||
curr_tot_sample = -(float)ADC_curr_norm_value[0];
|
||||
}
|
||||
} else {
|
||||
curr_tot_sample = (float)ADC_curr_norm_value[1];
|
||||
if (direction) {
|
||||
switch (comm_step) {
|
||||
case 1: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
|
||||
case 2: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
|
||||
case 3: curr_tot_sample = (float)ADC_curr_norm_value[0]; break;
|
||||
case 4: curr_tot_sample = (float)ADC_curr_norm_value[1]; break;
|
||||
case 5: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
|
||||
case 6: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
|
||||
default: break;
|
||||
}
|
||||
break;
|
||||
|
||||
case 2:
|
||||
case 3:
|
||||
curr_tot_sample = (float)ADC_curr_norm_value[0];
|
||||
break;
|
||||
|
||||
case 4:
|
||||
case 5:
|
||||
if (direction) {
|
||||
curr_tot_sample = (float)ADC_curr_norm_value[1];
|
||||
} else {
|
||||
if (comm_step == 4) {
|
||||
curr_tot_sample = -(float)ADC_curr_norm_value[1];
|
||||
} else {
|
||||
curr_tot_sample = -(float)ADC_curr_norm_value[0];
|
||||
}
|
||||
} else {
|
||||
switch (comm_step) {
|
||||
case 1: curr_tot_sample = (float)ADC_curr_norm_value[1]; break;
|
||||
case 2: curr_tot_sample = (float)ADC_curr_norm_value[0]; break;
|
||||
case 3: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
|
||||
case 4: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
|
||||
case 5: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
|
||||
case 6: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
|
||||
default: break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
const float tot_sample_tmp = curr_tot_sample;
|
||||
static int comm_step_prev = 1;
|
||||
static float prev_tot_sample = 0.0;
|
||||
if (comm_step != comm_step_prev) {
|
||||
curr_tot_sample = prev_tot_sample;
|
||||
}
|
||||
comm_step_prev = comm_step;
|
||||
prev_tot_sample = tot_sample_tmp;
|
||||
}
|
||||
|
||||
if (detect_now == 4) {
|
||||
float a = fabsf(ADC_curr_norm_value[0]);
|
||||
float b = fabsf(ADC_curr_norm_value[1]);
|
||||
const float a = fabsf(ADC_curr_norm_value[0]);
|
||||
const float b = fabsf(ADC_curr_norm_value[1]);
|
||||
|
||||
if (a > b) {
|
||||
mcpwm_detect_currents[detect_step] = a;
|
||||
|
|
@ -1672,8 +1721,8 @@ void mcpwm_adc_inj_int_handler(void) {
|
|||
mcpwm_detect_currents[5] - mcpwm_detect_currents[detect_step];
|
||||
}
|
||||
|
||||
int vzero = ADC_V_ZERO;
|
||||
// int vzero = (ADC_V_L1 + ADC_V_L2 + ADC_V_L3) / 3;
|
||||
const int vzero = ADC_V_ZERO;
|
||||
// const int vzero = (ADC_V_L1 + ADC_V_L2 + ADC_V_L3) / 3;
|
||||
|
||||
switch (comm_step) {
|
||||
case 1:
|
||||
|
|
@ -1722,8 +1771,14 @@ void mcpwm_adc_inj_int_handler(void) {
|
|||
}
|
||||
}
|
||||
|
||||
last_current_sample = curr_tot_sample;
|
||||
filter_add_sample((float*) current_fir_samples, curr_tot_sample,
|
||||
last_current_sample = curr_tot_sample * (V_REG / 4095.0) / (CURRENT_SHUNT_RES * CURRENT_AMP_GAIN);
|
||||
|
||||
// Filter out outliers
|
||||
if (fabsf(last_current_sample) > (conf.l_abs_current_max * 1.2)) {
|
||||
last_current_sample = SIGN(last_current_sample) * conf.l_abs_current_max * 1.2;
|
||||
}
|
||||
|
||||
filter_add_sample((float*) current_fir_samples, last_current_sample,
|
||||
CURR_FIR_TAPS_BITS, (uint32_t*) ¤t_fir_index);
|
||||
last_current_sample_filtered = filter_run_fir_iteration(
|
||||
(float*) current_fir_samples, (float*) current_fir_coeffs,
|
||||
|
|
@ -1761,7 +1816,7 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags) {
|
|||
direction_before = direction;
|
||||
|
||||
// Check for faults that should stop the motor
|
||||
static float wrong_voltage_iterations = 0;
|
||||
static int wrong_voltage_iterations = 0;
|
||||
if (input_voltage < conf.l_min_vin ||
|
||||
input_voltage > conf.l_max_vin) {
|
||||
wrong_voltage_iterations++;
|
||||
|
|
@ -1833,7 +1888,7 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags) {
|
|||
}
|
||||
}
|
||||
|
||||
if ((state == MC_STATE_RUNNING && pwm_cycles_sum >= 2.0) || state == MC_STATE_OFF) {
|
||||
if ((state == MC_STATE_RUNNING && pwm_cycles >= 2.0) || state == MC_STATE_OFF) {
|
||||
int v_diff = 0;
|
||||
int ph_now_raw = 0;
|
||||
|
||||
|
|
@ -1923,8 +1978,9 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags) {
|
|||
}
|
||||
|
||||
pwm_cycles_sum += (float)MCPWM_SWITCH_FREQUENCY_MAX / switching_frequency_now;
|
||||
pwm_cycles++;
|
||||
} else {
|
||||
int hall_phase = mcpwm_read_hall_phase();
|
||||
const int hall_phase = mcpwm_read_hall_phase();
|
||||
if (comm_step != hall_phase) {
|
||||
comm_step = hall_phase;
|
||||
|
||||
|
|
@ -2390,6 +2446,10 @@ static void update_adc_sample_pos(mc_timer_struct *timer_tmp) {
|
|||
volatile uint32_t curr1_sample = timer_tmp->curr1_sample;
|
||||
volatile uint32_t curr2_sample = timer_tmp->curr2_sample;
|
||||
|
||||
if (duty > (uint32_t)((float)top * conf.l_max_duty)) {
|
||||
duty = (uint32_t)((float)top * conf.l_max_duty);
|
||||
}
|
||||
|
||||
curr_samp_volt = 0;
|
||||
|
||||
if (conf.motor_type == MOTOR_TYPE_DC) {
|
||||
|
|
@ -2501,17 +2561,59 @@ static void update_adc_sample_pos(mc_timer_struct *timer_tmp) {
|
|||
val_sample = duty / 2;
|
||||
|
||||
// Current samples
|
||||
curr1_sample = duty + (top - duty) / 2 + 1000;
|
||||
if (curr1_sample > (top - 20)) {
|
||||
curr1_sample = top - 20;
|
||||
curr1_sample = duty + (top - duty) / 2;
|
||||
if (curr1_sample > (top - 70)) {
|
||||
curr1_sample = top - 70;
|
||||
}
|
||||
|
||||
// curr1_sample = duty + 1500;
|
||||
// curr1_sample = duty + (top - duty) / 2;
|
||||
// curr1_sample = duty + 2 * (top - duty) / 3;
|
||||
// curr1_sample = top - 20;
|
||||
|
||||
curr2_sample = curr1_sample;
|
||||
|
||||
// The off sampling time is short, so use the on sampling time
|
||||
// where possible
|
||||
if (duty > (top / 2)) {
|
||||
#if CURR1_DOUBLE_SAMPLE
|
||||
if (comm_step == 2 || comm_step == 3) {
|
||||
curr1_sample = duty + 90;
|
||||
curr2_sample = top - 230;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if CURR2_DOUBLE_SAMPLE
|
||||
if (direction) {
|
||||
if (comm_step == 4 || comm_step == 5) {
|
||||
curr1_sample = duty + 90;
|
||||
curr2_sample = top - 230;
|
||||
}
|
||||
} else {
|
||||
if (comm_step == 1 || comm_step == 6) {
|
||||
curr1_sample = duty + 90;
|
||||
curr2_sample = top - 230;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
if (direction) {
|
||||
switch (comm_step) {
|
||||
case 1: curr_samp_volt = 3; break;
|
||||
case 2: curr_samp_volt = 2; break;
|
||||
case 3: curr_samp_volt = 2; break;
|
||||
case 4: curr_samp_volt = 1; break;
|
||||
case 5: curr_samp_volt = 1; break;
|
||||
case 6: curr_samp_volt = 3; break;
|
||||
default: break;
|
||||
}
|
||||
} else {
|
||||
switch (comm_step) {
|
||||
case 1: curr_samp_volt = 1; break;
|
||||
case 2: curr_samp_volt = 2; break;
|
||||
case 3: curr_samp_volt = 2; break;
|
||||
case 4: curr_samp_volt = 3; break;
|
||||
case 5: curr_samp_volt = 3; break;
|
||||
case 6: curr_samp_volt = 1; break;
|
||||
default: break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -2564,6 +2666,7 @@ static void commutate(int steps) {
|
|||
last_pwm_cycles_sum = pwm_cycles_sum;
|
||||
last_pwm_cycles_sums[comm_step - 1] = pwm_cycles_sum;
|
||||
pwm_cycles_sum = 0;
|
||||
pwm_cycles = 0;
|
||||
|
||||
if (conf.motor_type == MOTOR_TYPE_BLDC && sensorless_now) {
|
||||
comm_step += steps;
|
||||
|
|
|
|||
10
mcpwm.h
10
mcpwm.h
|
|
@ -90,7 +90,7 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags);
|
|||
extern volatile uint16_t ADC_Value[];
|
||||
extern volatile int ADC_curr_norm_value[];
|
||||
extern volatile float mcpwm_detect_currents[];
|
||||
extern volatile float mcpwm_detect_voltages[6];
|
||||
extern volatile float mcpwm_detect_voltages[];
|
||||
extern volatile float mcpwm_detect_currents_diff[];
|
||||
extern volatile int mcpwm_vzero;
|
||||
|
||||
|
|
@ -98,13 +98,13 @@ extern volatile int mcpwm_vzero;
|
|||
* Fixed parameters
|
||||
*/
|
||||
#define MCPWM_SWITCH_FREQUENCY_MIN 3000 // The lowest switching frequency in Hz
|
||||
#define MCPWM_SWITCH_FREQUENCY_MAX 35000 // The highest switching frequency in Hz
|
||||
#define MCPWM_SWITCH_FREQUENCY_MAX 40000 // The highest switching frequency in Hz
|
||||
#define MCPWM_SWITCH_FREQUENCY_DC_MOTOR 25000 // The DC motor switching frequency
|
||||
#define MCPWM_DEAD_TIME_CYCLES 80 // Dead time
|
||||
#define MCPWM_DEAD_TIME_CYCLES 100 // Dead time
|
||||
#define MCPWM_RPM_TIMER_FREQ 1000000.0 // Frequency of the RPM measurement timer
|
||||
#define MCPWM_RAMP_STEP 0.01 // Ramping step (1000 times/sec) at maximum duty cycle
|
||||
#define MCPWM_RAMP_STEP 0.02 // Ramping step (1000 times/sec) at maximum duty cycle
|
||||
#define MCPWM_RAMP_STEP_RPM_LIMIT 0.0005 // Ramping step when limiting the RPM
|
||||
#define MCPWM_CURRENT_LIMIT_GAIN 2.0 // The error gain of the current limiting algorithm
|
||||
#define MCPWM_CURRENT_LIMIT_GAIN 0.7 // The error gain of the current limiting algorithm
|
||||
#define MCPWM_CMD_STOP_TIME 0 // Ignore commands for this duration in msec after a stop has been sent
|
||||
#define MCPWM_DETECT_STOP_TIME 500 // Ignore commands for this duration in msec after a detect command
|
||||
|
||||
|
|
|
|||
27
servo_dec.c
27
servo_dec.c
|
|
@ -27,6 +27,7 @@
|
|||
#include "ch.h"
|
||||
#include "hal.h"
|
||||
#include "hw.h"
|
||||
#include "utils.h"
|
||||
|
||||
/*
|
||||
* Settings
|
||||
|
|
@ -45,39 +46,33 @@ static volatile bool use_median_filter = false;
|
|||
// Function pointers
|
||||
static void(*done_func)(void) = 0;
|
||||
|
||||
static float middle_of_3(float a, float b, float c) {
|
||||
float middle;
|
||||
|
||||
if ((a <= b) && (a <= c)) {
|
||||
middle = (b <= c) ? b : c;
|
||||
} else if ((b <= a) && (b <= c)) {
|
||||
middle = (a <= c) ? a : c;
|
||||
} else {
|
||||
middle = (a <= b) ? a : b;
|
||||
}
|
||||
return middle;
|
||||
}
|
||||
|
||||
static void icuwidthcb(ICUDriver *icup) {
|
||||
last_len_received[0] = ((float)icuGetWidth(icup) / ((float)TIMER_FREQ / 1000.0));
|
||||
float len = last_len_received[0] - pulse_start;
|
||||
const float len_set = (pulse_end - pulse_start);
|
||||
|
||||
if (len > len_set) {
|
||||
if (len < len_set * 1.2) {
|
||||
if (len < (len_set * 1.2)) {
|
||||
len = len_set;
|
||||
} else {
|
||||
// Too long pulse. Most likely something is wrong.
|
||||
len = -1.0;
|
||||
}
|
||||
} else if (len < 0.0) {
|
||||
if ((len + pulse_start) > (pulse_start * 0.8)) {
|
||||
len = 0.0;
|
||||
} else {
|
||||
// Too short pulse. Most likely something is wrong.
|
||||
len = -1.0;
|
||||
}
|
||||
}
|
||||
|
||||
if (len > 0.0) {
|
||||
if (len >= 0.0) {
|
||||
if (use_median_filter) {
|
||||
float c = (len * 2.0 - len_set) / len_set;
|
||||
static float c1 = 0.5;
|
||||
static float c2 = 0.5;
|
||||
float med = middle_of_3(c, c1, c2);
|
||||
float med = utils_middle_of_3(c, c1, c2);
|
||||
|
||||
c2 = c1;
|
||||
c1 = c;
|
||||
|
|
|
|||
|
|
@ -35,7 +35,7 @@
|
|||
#include <stdio.h>
|
||||
|
||||
// Private variables
|
||||
#define FAULT_VEC_LEN 30
|
||||
#define FAULT_VEC_LEN 25
|
||||
static volatile fault_data fault_vec[FAULT_VEC_LEN];
|
||||
static volatile int fault_vec_write = 0;
|
||||
|
||||
|
|
@ -101,8 +101,11 @@ void terminal_process_string(char *str) {
|
|||
commands_printf("Duty : %.2f", (double)fault_vec[i].duty);
|
||||
commands_printf("RPM : %.1f", (double)fault_vec[i].rpm);
|
||||
commands_printf("Tacho : %d", fault_vec[i].tacho);
|
||||
commands_printf("TIM PWM CNT : %d", fault_vec[i].tim_pwm_cnt);
|
||||
commands_printf("TIM Samp CNT : %d", fault_vec[i].tim_samp_cnt);
|
||||
commands_printf("Cycles running : %d", fault_vec[i].cycles_running);
|
||||
commands_printf("TIM duty : %d", (int)((float)fault_vec[i].tim_top * fault_vec[i].duty));
|
||||
commands_printf("TIM val samp : %d", fault_vec[i].tim_val_samp);
|
||||
commands_printf("TIM current samp : %d", fault_vec[i].tim_current_samp);
|
||||
commands_printf("TIM top : %d", fault_vec[i].tim_top);
|
||||
commands_printf("Comm step : %d", fault_vec[i].comm_step);
|
||||
commands_printf("Temperature : %.2f\n", (double)fault_vec[i].temperature);
|
||||
}
|
||||
|
|
|
|||
60
utils.c
60
utils.c
|
|
@ -81,6 +81,10 @@ float utils_map(float x, float in_min, float in_max, float out_min, float out_ma
|
|||
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
||||
}
|
||||
|
||||
int utils_map_int(int x, int in_min, int in_max, int out_min, int out_max) {
|
||||
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
||||
}
|
||||
|
||||
/**
|
||||
* Truncate absolute values less than tres to zero. The value
|
||||
* tres will be mapped to 0 and the value max to max.
|
||||
|
|
@ -123,6 +127,62 @@ float utils_angle_difference(float angle1, float angle2) {
|
|||
return angle1 - angle2;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the middle value of three values
|
||||
*
|
||||
* @param a
|
||||
* First value
|
||||
*
|
||||
* @param b
|
||||
* Second value
|
||||
*
|
||||
* @param c
|
||||
* Third value
|
||||
*
|
||||
* @return
|
||||
* The middle value
|
||||
*/
|
||||
float utils_middle_of_3(float a, float b, float c) {
|
||||
float middle;
|
||||
|
||||
if ((a <= b) && (a <= c)) {
|
||||
middle = (b <= c) ? b : c;
|
||||
} else if ((b <= a) && (b <= c)) {
|
||||
middle = (a <= c) ? a : c;
|
||||
} else {
|
||||
middle = (a <= b) ? a : b;
|
||||
}
|
||||
return middle;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the middle value of three values
|
||||
*
|
||||
* @param a
|
||||
* First value
|
||||
*
|
||||
* @param b
|
||||
* Second value
|
||||
*
|
||||
* @param c
|
||||
* Third value
|
||||
*
|
||||
* @return
|
||||
* The middle value
|
||||
*/
|
||||
int utils_middle_of_3_int(int a, int b, int c) {
|
||||
int middle;
|
||||
|
||||
if ((a <= b) && (a <= c)) {
|
||||
middle = (b <= c) ? b : c;
|
||||
} else if ((b <= a) && (b <= c)) {
|
||||
middle = (a <= c) ? a : c;
|
||||
} else {
|
||||
middle = (a <= b) ? a : b;
|
||||
}
|
||||
return middle;
|
||||
}
|
||||
|
||||
/**
|
||||
* A system locking function with a counter. For every lock, a corresponding unlock must
|
||||
* exist to unlock the system. That means, if lock is called five times, unlock has to
|
||||
|
|
|
|||
3
utils.h
3
utils.h
|
|
@ -30,8 +30,11 @@ float utils_calc_ratio(float low, float high, float val);
|
|||
void utils_norm_angle(float *angle);
|
||||
int utils_truncate_number(float *number, float min, float max);
|
||||
float utils_map(float x, float in_min, float in_max, float out_min, float out_max);
|
||||
int utils_map_int(int x, int in_min, int in_max, int out_min, int out_max);
|
||||
void utils_deadband(float *value, float tres, float max);
|
||||
float utils_angle_difference(float angle1, float angle2);
|
||||
float utils_middle_of_3(float a, float b, float c);
|
||||
int utils_middle_of_3_int(int a, int b, int c);
|
||||
void utils_sys_lock_cnt(void);
|
||||
void utils_sys_unlock_cnt(void);
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue