diff --git a/Makefile b/Makefile
index ee4ba0de2..de5ed8c38 100644
--- a/Makefile
+++ b/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).
diff --git a/applications/app.c b/applications/app.c
index 1bb809311..ba3935828 100644
--- a/applications/app.c
+++ b/applications/app.c
@@ -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;
diff --git a/applications/app.h b/applications/app.h
index 771516979..3b0c66f8e 100644
--- a/applications/app.h
+++ b/applications/app.h
@@ -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);
diff --git a/applications/app_adc.c b/applications/app_adc.c
index 7bb93528e..77034c404 100644
--- a/applications/app_adc.c
+++ b/applications/app_adc.c
@@ -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;
diff --git a/applications/app_nunchuk.c b/applications/app_nunchuk.c
index 5e9ec4a5d..847057c4b 100644
--- a/applications/app_nunchuk.c
+++ b/applications/app_nunchuk.c
@@ -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];
}
diff --git a/commands.c b/commands.c
index d30b83f1b..8c247d204 100644
--- a/commands.c
+++ b/commands.c
@@ -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);
diff --git a/conf_general.c b/conf_general.c
index 31ce45caf..bc4c4ca20 100644
--- a/conf_general.c
+++ b/conf_general.c
@@ -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;
diff --git a/conf_general.h b/conf_general.h
index 33331d9bf..69a1439c8 100644
--- a/conf_general.h
+++ b/conf_general.h
@@ -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);
diff --git a/datatypes.h b/datatypes.h
index acb69a3d9..b14564895 100644
--- a/datatypes.h
+++ b/datatypes.h
@@ -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;
diff --git a/hwconf/hw_40.c b/hwconf/hw_40.c
index 95f753638..0f1b90349 100644
--- a/hwconf/hw_40.c
+++ b/hwconf/hw_40.c
@@ -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) {
diff --git a/hwconf/hw_40.h b/hwconf/hw_40.h
index e3d163d9e..caf72f54d 100644
--- a/hwconf/hw_40.h
+++ b/hwconf/hw_40.h
@@ -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
diff --git a/hwconf/hw_45.c b/hwconf/hw_45.c
index 7a4e18464..e4b7fdb41 100644
--- a/hwconf/hw_45.c
+++ b/hwconf/hw_45.c
@@ -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);
diff --git a/hwconf/hw_45.h b/hwconf/hw_45.h
index 924d520f8..b15523b7f 100644
--- a/hwconf/hw_45.h
+++ b/hwconf/hw_45.h
@@ -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
diff --git a/hwconf/hw_46.c b/hwconf/hw_46.c
index 62e550b28..21a845069 100644
--- a/hwconf/hw_46.c
+++ b/hwconf/hw_46.c
@@ -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) {
diff --git a/hwconf/hw_46.h b/hwconf/hw_46.h
index 3011c1487..1a5f41625 100644
--- a/hwconf/hw_46.h
+++ b/hwconf/hw_46.h
@@ -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
diff --git a/hwconf/hw_bw.c b/hwconf/hw_bw.c
deleted file mode 100644
index cc6d534d7..000000000
--- a/hwconf/hw_bw.c
+++ /dev/null
@@ -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 .
- */
-
-/*
- * 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
diff --git a/hwconf/hw_bw.h b/hwconf/hw_bw.h
deleted file mode 100644
index d0aa68d01..000000000
--- a/hwconf/hw_bw.h
+++ /dev/null
@@ -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 .
- */
-
-/*
- * 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_ */
diff --git a/hwconf/hw_r2.c b/hwconf/hw_r2.c
index 3c949da36..a6334fb6e 100644
--- a/hwconf/hw_r2.c
+++ b/hwconf/hw_r2.c
@@ -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) {
diff --git a/hwconf/hw_r2.h b/hwconf/hw_r2.h
index 2d24c4e70..54073b1e2 100644
--- a/hwconf/hw_r2.h
+++ b/hwconf/hw_r2.h
@@ -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
diff --git a/hwconf/hw_victor_r1a.c b/hwconf/hw_victor_r1a.c
index d70a6a1b9..04c9c316e 100644
--- a/hwconf/hw_victor_r1a.c
+++ b/hwconf/hw_victor_r1a.c
@@ -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) {
diff --git a/hwconf/hw_victor_r1a.h b/hwconf/hw_victor_r1a.h
index d87ebca3e..03e758642 100644
--- a/hwconf/hw_victor_r1a.h
+++ b/hwconf/hw_victor_r1a.h
@@ -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
diff --git a/hwconf/hwconf.mk b/hwconf/hwconf.mk
index ebbec9e68..9c69520a0 100644
--- a/hwconf/hwconf.mk
+++ b/hwconf/hwconf.mk
@@ -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
diff --git a/main.c b/main.c
index dc0054dac..d844d758a 100644
--- a/main.c
+++ b/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);
diff --git a/mcpwm.c b/mcpwm.c
index 64b9b3081..38afb49b4 100644
--- a/mcpwm.c
+++ b/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;
diff --git a/mcpwm.h b/mcpwm.h
index b121c3f83..a87874fd7 100644
--- a/mcpwm.h
+++ b/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
diff --git a/servo_dec.c b/servo_dec.c
index 74596018d..bd670738b 100644
--- a/servo_dec.c
+++ b/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;
diff --git a/terminal.c b/terminal.c
index 873f1c547..fd746f4bc 100644
--- a/terminal.c
+++ b/terminal.c
@@ -35,7 +35,7 @@
#include
// 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);
}
diff --git a/utils.c b/utils.c
index 332d8a987..0a3c8d3fc 100644
--- a/utils.c
+++ b/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
diff --git a/utils.h b/utils.h
index e99b053e4..b9d4cd9c5 100644
--- a/utils.h
+++ b/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);