diff --git a/Makefile b/Makefile
index d2d26220..5548c678 100644
--- a/Makefile
+++ b/Makefile
@@ -270,8 +270,8 @@ build/$(PROJECT).bin: build/$(PROJECT).elf
# Program
upload: build/$(PROJECT).bin
#qstlink2 --cli --erase --write build/$(PROJECT).bin
- openocd -f interface/stlink-v2.cfg -c "set WORKAREASIZE 0x2000" -f target/stm32f4x_stlink.cfg -c "program build/$(PROJECT).elf verify reset"
- #openocd -f board/stm32f4discovery.cfg -c "reset_config trst_only combined" -c "program build/$(PROJECT).elf verify reset exit" # For openocd 0.9
+ #openocd -f interface/stlink-v2.cfg -c "set WORKAREASIZE 0x2000" -f target/stm32f4x_stlink.cfg -c "program build/$(PROJECT).elf verify reset" # Older openocd
+ openocd -f board/stm32f4discovery.cfg -c "reset_config trst_only combined" -c "program build/$(PROJECT).elf verify reset exit" # For openocd 0.9
#program with olimex arm-usb-tiny-h and jtag-swd adapter board. needs openocd>=0.9
upload-olimex: build/$(PROJECT).bin
diff --git a/appconf/appconf_custom.h b/appconf/appconf_custom.h
new file mode 100644
index 00000000..ed73a846
--- /dev/null
+++ b/appconf/appconf_custom.h
@@ -0,0 +1,31 @@
+/*
+ Copyright 2016 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 .
+ */
+
+/*
+ * appconf_example_ppm.h
+ *
+ * Created on: 9 sep 2015
+ * Author: benjamin
+ */
+
+#ifndef APPCONF_APPCONF_EXAMPLE_PPM_H_
+#define APPCONF_APPCONF_EXAMPLE_PPM_H_
+
+// Use custom user application
+#define APPCONF_APP_TO_USE APP_CUSTOM
+
+#endif
diff --git a/applications/app_sten.c b/applications/app_sten.c
index 7e107dce..b4caeeed 100644
--- a/applications/app_sten.c
+++ b/applications/app_sten.c
@@ -32,6 +32,8 @@
#include "utils.h"
#include "hw.h"
#include "timeout.h"
+#include "comm_can.h"
+
#include
// Settings
@@ -147,15 +149,7 @@ static THD_FUNCTION(uart_thread, arg) {
}
static void set_output(float output) {
- output /= (1.0 - HYST);
-
- if (output > HYST) {
- output -= HYST;
- } else if (output < -HYST) {
- output += HYST;
- } else {
- output = 0.0;
- }
+ utils_deadband(&output, HYST, 1.0);
const float rpm = mc_interface_get_rpm();
@@ -182,13 +176,11 @@ static void set_output(float output) {
current_p2 = current_p1;
current_p1 = current;
- if (fabsf(current) < mc_interface_get_configuration()->cc_min_current) {
- current = -mc_interface_get_configuration()->cc_min_current;
- }
-
mc_interface_set_current(current);
+ comm_can_set_current(255, current);
} else {
mc_interface_set_brake_current(output * mc_interface_get_configuration()->l_current_min);
+ comm_can_set_current_brake(255, output * mc_interface_get_configuration()->l_current_min);
}
}
diff --git a/commands.c b/commands.c
index fc1be0af..169296fe 100644
--- a/commands.c
+++ b/commands.c
@@ -314,7 +314,7 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
mcconf.foc_sl_d_current_factor = buffer_get_float32(data, 1e3, &ind);
memcpy(mcconf.foc_hall_table, data + ind, 8);
ind += 8;
- mcconf.foc_hall_sl_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
+ mcconf.foc_sl_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.s_pid_kp = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.s_pid_ki = (float)buffer_get_int32(data, &ind) / 1000000.0;
@@ -336,14 +336,11 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
mcconf.m_duty_ramp_step_rpm_lim = (float)buffer_get_float32(data, 1000000.0, &ind);
mcconf.m_current_backoff_gain = (float)buffer_get_float32(data, 1000000.0, &ind);
mcconf.m_encoder_counts = buffer_get_uint32(data, &ind);
+ mcconf.m_sensor_port_mode = data[ind++];
conf_general_store_mc_configuration(&mcconf);
mc_interface_set_configuration(&mcconf);
-#if ENCODER_ENABLE
- encoder_set_counts(mcconf.m_encoder_counts);
-#endif
-
ind = 0;
send_buffer[ind++] = packet_id;
commands_send_packet(send_buffer, ind);
@@ -422,7 +419,7 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
buffer_append_float32(send_buffer, mcconf.foc_sl_d_current_factor, 1e3, &ind);
memcpy(send_buffer + ind, mcconf.foc_hall_table, 8);
ind += 8;
- buffer_append_int32(send_buffer, (int32_t)(mcconf.foc_hall_sl_erpm * 1000.0), &ind);
+ buffer_append_int32(send_buffer, (int32_t)(mcconf.foc_sl_erpm * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_kp * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_ki * 1000000.0), &ind);
@@ -444,6 +441,7 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
buffer_append_float32(send_buffer, mcconf.m_duty_ramp_step_rpm_lim, 1000000.0, &ind);
buffer_append_float32(send_buffer, mcconf.m_current_backoff_gain, 1000000.0, &ind);
buffer_append_uint32(send_buffer, mcconf.m_encoder_counts, &ind);
+ send_buffer[ind++] = mcconf.m_sensor_port_mode;
commands_send_packet(send_buffer, ind);
break;
@@ -663,66 +661,74 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
break;
case COMM_DETECT_ENCODER: {
-#if ENCODER_ENABLE
- mcconf = *mc_interface_get_configuration();
- mcconf_old = mcconf;
+ if (encoder_is_configured()) {
+ mcconf = *mc_interface_get_configuration();
+ mcconf_old = mcconf;
- ind = 0;
- float current = buffer_get_float32(data, 1e3, &ind);
+ ind = 0;
+ float current = buffer_get_float32(data, 1e3, &ind);
- mcconf.motor_type = MOTOR_TYPE_FOC;
- mcconf.foc_f_sw = 10000.0;
- mcconf.foc_current_kp = 0.01;
- mcconf.foc_current_ki = 10.0;
- mc_interface_set_configuration(&mcconf);
+ mcconf.motor_type = MOTOR_TYPE_FOC;
+ mcconf.foc_f_sw = 10000.0;
+ mcconf.foc_current_kp = 0.01;
+ mcconf.foc_current_ki = 10.0;
+ mc_interface_set_configuration(&mcconf);
- float offset = 0.0;
- float ratio = 0.0;
- bool inverted = false;
- mcpwm_foc_encoder_detect(current, false, &offset, &ratio, &inverted);
- mc_interface_set_configuration(&mcconf_old);
+ float offset = 0.0;
+ float ratio = 0.0;
+ bool inverted = false;
+ mcpwm_foc_encoder_detect(current, false, &offset, &ratio, &inverted);
+ mc_interface_set_configuration(&mcconf_old);
- ind = 0;
- send_buffer[ind++] = COMM_DETECT_ENCODER;
- buffer_append_float32(send_buffer, offset, 1e6, &ind);
- buffer_append_float32(send_buffer, ratio, 1e6, &ind);
- send_buffer[ind++] = inverted;
- commands_send_packet(send_buffer, ind);
-#else
- ind = 0;
- send_buffer[ind++] = COMM_DETECT_ENCODER;
- buffer_append_float32(send_buffer, 1001.0, 1e6, &ind);
- buffer_append_float32(send_buffer, 0.0, 1e6, &ind);
- send_buffer[ind++] = false;
- commands_send_packet(send_buffer, ind);
-#endif
+ ind = 0;
+ send_buffer[ind++] = COMM_DETECT_ENCODER;
+ buffer_append_float32(send_buffer, offset, 1e6, &ind);
+ buffer_append_float32(send_buffer, ratio, 1e6, &ind);
+ send_buffer[ind++] = inverted;
+ commands_send_packet(send_buffer, ind);
+ } else {
+ ind = 0;
+ send_buffer[ind++] = COMM_DETECT_ENCODER;
+ buffer_append_float32(send_buffer, 1001.0, 1e6, &ind);
+ buffer_append_float32(send_buffer, 0.0, 1e6, &ind);
+ send_buffer[ind++] = false;
+ commands_send_packet(send_buffer, ind);
+ }
}
break;
case COMM_DETECT_HALL_FOC: {
mcconf = *mc_interface_get_configuration();
- mcconf_old = mcconf;
- ind = 0;
- float current = buffer_get_float32(data, 1e3, &ind);
+ if (mcconf.m_sensor_port_mode == SENSOR_PORT_MODE_HALL) {
+ mcconf_old = mcconf;
+ ind = 0;
+ float current = buffer_get_float32(data, 1e3, &ind);
- mcconf.motor_type = MOTOR_TYPE_FOC;
- mcconf.foc_f_sw = 10000.0;
- mcconf.foc_current_kp = 0.01;
- mcconf.foc_current_ki = 10.0;
- mc_interface_set_configuration(&mcconf);
+ mcconf.motor_type = MOTOR_TYPE_FOC;
+ mcconf.foc_f_sw = 10000.0;
+ mcconf.foc_current_kp = 0.01;
+ mcconf.foc_current_ki = 10.0;
+ mc_interface_set_configuration(&mcconf);
- uint8_t hall_tab[8];
- bool res = mcpwm_foc_hall_detect(current, hall_tab);
- mc_interface_set_configuration(&mcconf_old);
+ uint8_t hall_tab[8];
+ bool res = mcpwm_foc_hall_detect(current, hall_tab);
+ mc_interface_set_configuration(&mcconf_old);
- ind = 0;
- send_buffer[ind++] = COMM_DETECT_HALL_FOC;
- memcpy(send_buffer + ind, hall_tab, 8);
- ind += 8;
- send_buffer[ind++] = res ? 0 : 1;
+ ind = 0;
+ send_buffer[ind++] = COMM_DETECT_HALL_FOC;
+ memcpy(send_buffer + ind, hall_tab, 8);
+ ind += 8;
+ send_buffer[ind++] = res ? 0 : 1;
- commands_send_packet(send_buffer, ind);
+ commands_send_packet(send_buffer, ind);
+ } else {
+ ind = 0;
+ send_buffer[ind++] = COMM_DETECT_HALL_FOC;
+ memset(send_buffer, 255, 8);
+ ind += 8;
+ send_buffer[ind++] = 0;
+ }
}
break;
diff --git a/conf_general.c b/conf_general.c
index b2a04556..a7fe6eaa 100644
--- a/conf_general.c
+++ b/conf_general.c
@@ -218,7 +218,7 @@ void conf_general_get_default_mc_configuration(mc_configuration *conf) {
conf->foc_hall_table[5] = MCCONF_FOC_HALL_TAB_5;
conf->foc_hall_table[6] = MCCONF_FOC_HALL_TAB_6;
conf->foc_hall_table[7] = MCCONF_FOC_HALL_TAB_7;
- conf->foc_hall_sl_erpm = MCCONF_FOC_HALL_ERPM;
+ conf->foc_sl_erpm = MCCONF_FOC_SL_ERPM;
conf->s_pid_kp = MCCONF_S_PID_KP;
conf->s_pid_ki = MCCONF_S_PID_KI;
@@ -240,6 +240,7 @@ void conf_general_get_default_mc_configuration(mc_configuration *conf) {
conf->m_duty_ramp_step_rpm_lim = MCCONF_M_RAMP_STEP_RPM_LIM;
conf->m_current_backoff_gain = MCCONF_M_CURRENT_BACKOFF_GAIN;
conf->m_encoder_counts = MCCONF_M_ENCODER_COUNTS;
+ conf->m_sensor_port_mode = MCCONF_M_SENSOR_PORT_MODE;
}
/**
diff --git a/conf_general.h b/conf_general.h
index a2e3c792..449c9fe8 100644
--- a/conf_general.h
+++ b/conf_general.h
@@ -27,7 +27,7 @@
// Firmware version
#define FW_VERSION_MAJOR 2
-#define FW_VERSION_MINOR 16
+#define FW_VERSION_MINOR 17
#include "datatypes.h"
@@ -38,6 +38,7 @@
// Settings and parameters to override
//#define VIN_R1 33000.0
+//#define VIN_R1 39200.0
//#define VIN_R2 2200.0
//#define CURRENT_AMP_GAIN 10.0
//#define CURRENT_SHUNT_RES 0.0005
@@ -65,11 +66,13 @@
* Select default user motor configuration
*/
//#define MCCONF_DEFAULT_USER "mcconf_sten.h"
+//#define MCCONF_DEFAULT_USER "mcconf_sp_540kv.h"
/*
* Select default user app configuration
*/
//#define APPCONF_DEFAULT_USER "appconf_example_ppm.h"
+//#define APPCONF_DEFAULT_USER "appconf_custom.h"
/*
* Select which custom application to use. To configure the default applications and
@@ -78,13 +81,6 @@
*/
//#define USE_APP_STEN
-/*
- * Use encoder
- */
-#ifndef ENCODER_ENABLE
-#define ENCODER_ENABLE 0
-#endif
-
/*
* Enable CAN-bus
*/
diff --git a/datatypes.h b/datatypes.h
index 42b50843..91b23869 100644
--- a/datatypes.h
+++ b/datatypes.h
@@ -95,6 +95,12 @@ typedef enum {
DISP_POS_MODE_ENCODER_OBSERVER_ERROR
} disp_pos_mode;
+typedef enum {
+ SENSOR_PORT_MODE_HALL = 0,
+ SENSOR_PORT_MODE_ABI,
+ SENSOR_PORT_MODE_AS5047_SPI
+} sensor_port_mode;
+
typedef struct {
float cycle_int_limit;
float cycle_int_limit_running;
@@ -172,7 +178,7 @@ typedef struct {
float foc_sl_d_current_factor;
mc_foc_sensor_mode foc_sensor_mode;
uint8_t foc_hall_table[8];
- float foc_hall_sl_erpm;
+ float foc_sl_erpm;
// Speed PID
float s_pid_kp;
float s_pid_ki;
@@ -194,6 +200,7 @@ typedef struct {
float m_duty_ramp_step_rpm_lim;
float m_current_backoff_gain;
uint32_t m_encoder_counts;
+ sensor_port_mode m_sensor_port_mode;
} mc_configuration;
// Applications to use
diff --git a/encoder.c b/encoder.c
index bd4aa217..fcfa1f42 100644
--- a/encoder.c
+++ b/encoder.c
@@ -1,5 +1,5 @@
/*
- Copyright 2012-2015 Benjamin Vedder benjamin@vedder.se
+ Copyright 2012-2016 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
@@ -15,26 +15,64 @@
along with this program. If not, see .
*/
-/*
- * encoder.c
- *
- * Created on: 7 mar 2015
- * Author: benjamin
- */
-
#include "encoder.h"
#include "ch.h"
#include "hal.h"
#include "stm32f4xx_conf.h"
#include "hw.h"
+#include "utils.h"
+
+// Defines
+#define AS5047P_READ_ANGLECOM (0x3FFF | 0x4000 | 0x8000) // This is just ones
+#define AS5047_SAMPLE_RATE_HZ 20000
+
+#define SPI_SW_MISO_GPIO HW_HALL_ENC_GPIO2
+#define SPI_SW_MISO_PIN HW_HALL_ENC_PIN2
+#define SPI_SW_SCK_GPIO HW_HALL_ENC_GPIO1
+#define SPI_SW_SCK_PIN HW_HALL_ENC_PIN1
+#define SPI_SW_CS_GPIO HW_HALL_ENC_GPIO3
+#define SPI_SW_CS_PIN HW_HALL_ENC_PIN3
+
+// Private types
+typedef enum {
+ ENCODER_MODE_NONE = 0,
+ ENCODER_MODE_ABI,
+ ENCODER_MODE_AS5047P_SPI
+} encoder_mode;
// Private variables
-static bool index_found;
+static bool index_found = false;
static uint32_t enc_counts = 10000;
+static encoder_mode mode = ENCODER_MODE_NONE;
+static float last_enc_angle = 0.0;
-void encoder_init(uint32_t counts) {
+// Private functions
+uint16_t spi_exchange(uint16_t x);
+static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length);
+static void spi_begin(void);
+static void spi_end(void);
+static void spi_delay(void);
+
+void encoder_deinit(void) {
+ nvicDisableVector(HW_ENC_EXTI_CH);
+ nvicDisableVector(HW_ENC_TIM_ISR_CH);
+
+ TIM_DeInit(HW_ENC_TIM);
+
+ palSetPadMode(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN, PAL_MODE_INPUT_PULLUP);
+ palSetPadMode(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN, PAL_MODE_INPUT_PULLUP);
+ palSetPadMode(SPI_SW_CS_GPIO, SPI_SW_CS_PIN, PAL_MODE_INPUT_PULLUP);
+
+ 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);
+
+ index_found = false;
+ mode = ENCODER_MODE_NONE;
+ last_enc_angle = 0.0;
+}
+
+void encoder_init_abi(uint32_t counts) {
EXTI_InitTypeDef EXTI_InitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
// Initialize variables
index_found = false;
@@ -61,7 +99,7 @@ void encoder_init(uint32_t counts) {
TIM_ICPolarity_Rising);
TIM_SetAutoreload(HW_ENC_TIM, enc_counts - 1);
- TIM_Cmd (HW_ENC_TIM, ENABLE);
+ TIM_Cmd(HW_ENC_TIM, ENABLE);
// Interrupt on index pulse
@@ -76,15 +114,62 @@ void encoder_init(uint32_t counts) {
EXTI_Init(&EXTI_InitStructure);
// Enable and set EXTI Line Interrupt to the highest priority
- NVIC_InitStructure.NVIC_IRQChannel = HW_ENC_EXTI_CH;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x00;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x00;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
+ nvicEnableVector(HW_ENC_EXTI_CH, 0);
+
+ mode = ENCODER_MODE_ABI;
+}
+
+void encoder_init_as5047p_spi(void) {
+ TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+
+ palSetPadMode(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN, PAL_MODE_INPUT);
+ palSetPadMode(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+ palSetPadMode(SPI_SW_CS_GPIO, SPI_SW_CS_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+
+ // Enable timer clock
+ HW_ENC_TIM_CLK_EN();
+
+ // Time Base configuration
+ TIM_TimeBaseStructure.TIM_Prescaler = 0;
+ TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+ TIM_TimeBaseStructure.TIM_Period = ((168000000 / 2 / AS5047_SAMPLE_RATE_HZ) - 1);
+ TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+ TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
+ TIM_TimeBaseInit(HW_ENC_TIM, &TIM_TimeBaseStructure);
+
+ // Enable overflow interrupt
+ TIM_ITConfig(HW_ENC_TIM, TIM_IT_Update, ENABLE);
+
+ // Enable timer
+ TIM_Cmd(HW_ENC_TIM, ENABLE);
+
+ nvicEnableVector(HW_ENC_TIM_ISR_CH, 6);
+
+ mode = ENCODER_MODE_AS5047P_SPI;
+ index_found = true;
+}
+
+bool encoder_is_configured(void) {
+ return mode != ENCODER_MODE_NONE;
}
float encoder_read_deg(void) {
- return ((float)HW_ENC_TIM->CNT * 360.0) / (float)enc_counts;
+ static float angle = 0.0;
+
+ switch (mode) {
+ case ENCODER_MODE_ABI:
+ angle = ((float)HW_ENC_TIM->CNT * 360.0) / (float)enc_counts;
+ break;
+
+ case ENCODER_MODE_AS5047P_SPI:
+ angle = last_enc_angle;
+ break;
+
+ default:
+ break;
+ }
+
+ return angle;
}
/**
@@ -95,6 +180,20 @@ void encoder_reset(void) {
index_found = true;
}
+/**
+ * Timer interrupt
+ */
+void encoder_tim_isr(void) {
+ uint16_t pos;
+
+ spi_begin();
+ spi_transfer(&pos, 0, 1);
+ spi_end();
+
+ pos &= 0x3FFF;
+ last_enc_angle = ((float)pos * 360.0) / 16384.0;
+}
+
/**
* Set the number of encoder counts.
*
@@ -118,3 +217,53 @@ void encoder_set_counts(uint32_t counts) {
bool encoder_index_found(void) {
return index_found;
}
+
+// Software SPI
+uint16_t spi_exchange(uint16_t x) {
+ uint16_t rx;
+ spi_transfer(&rx, &x, 1);
+ return rx;
+}
+
+static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length) {
+ for (int i = 0;i < length;i++) {
+ uint16_t send = out_buf ? out_buf[i] : 0xFFFF;
+ uint16_t recieve = 0;
+
+ for (int bit = 0;bit < 16;bit++) {
+ //palWritePad(HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI, send >> 15);
+ send <<= 1;
+
+ spi_delay();
+ palSetPad(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN);
+ spi_delay();
+
+ recieve <<= 1;
+ if (palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN)) {
+ recieve |= 1;
+ }
+
+ palClearPad(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN);
+ spi_delay();
+ }
+
+ if (in_buf) {
+ in_buf[i] = recieve;
+ }
+ }
+}
+
+static void spi_begin(void) {
+ palClearPad(SPI_SW_CS_GPIO, SPI_SW_CS_PIN);
+}
+
+static void spi_end(void) {
+ palSetPad(SPI_SW_CS_GPIO, SPI_SW_CS_PIN);
+}
+
+static void spi_delay(void) {
+ __NOP();
+ __NOP();
+ __NOP();
+ __NOP();
+}
diff --git a/encoder.h b/encoder.h
index 0e73a925..360089a1 100644
--- a/encoder.h
+++ b/encoder.h
@@ -28,9 +28,13 @@
#include "conf_general.h"
// Functions
-void encoder_init(uint32_t counts);
+void encoder_deinit(void);
+void encoder_init_abi(uint32_t counts);
+void encoder_init_as5047p_spi(void);
+bool encoder_is_configured(void);
float encoder_read_deg(void);
void encoder_reset(void);
+void encoder_tim_isr(void);
void encoder_set_counts(uint32_t counts);
bool encoder_index_found(void);
diff --git a/halconf.h b/halconf.h
index 62096934..2e5e5ab2 100644
--- a/halconf.h
+++ b/halconf.h
@@ -146,7 +146,7 @@
* @brief Enables the SPI subsystem.
*/
#if !defined(HAL_USE_SPI) || defined(__DOXYGEN__)
-#define HAL_USE_SPI FALSE
+#define HAL_USE_SPI TRUE
#endif
/**
diff --git a/hwconf/hw_40.h b/hwconf/hw_40.h
index 9716400b..0773ccd8 100644
--- a/hwconf/hw_40.h
+++ b/hwconf/hw_40.h
@@ -146,6 +146,8 @@
#define HW_ENC_EXTI_CH EXTI9_5_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line8
#define HW_ENC_EXTI_ISR_VEC EXTI9_5_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN HW_ICU_GPIO
diff --git a/hwconf/hw_410.h b/hwconf/hw_410.h
index a61c6897..84611770 100644
--- a/hwconf/hw_410.h
+++ b/hwconf/hw_410.h
@@ -148,6 +148,8 @@
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line11
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN GPIOA
diff --git a/hwconf/hw_45.h b/hwconf/hw_45.h
index 74622dea..bb113177 100644
--- a/hwconf/hw_45.h
+++ b/hwconf/hw_45.h
@@ -146,6 +146,8 @@
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line11
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN HW_ICU_GPIO
diff --git a/hwconf/hw_46.h b/hwconf/hw_46.h
index 2b0d0de1..f807b224 100644
--- a/hwconf/hw_46.h
+++ b/hwconf/hw_46.h
@@ -154,6 +154,8 @@
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line11
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN HW_ICU_GPIO
diff --git a/hwconf/hw_48.h b/hwconf/hw_48.h
index 7c68e0f3..b7346408 100644
--- a/hwconf/hw_48.h
+++ b/hwconf/hw_48.h
@@ -148,6 +148,8 @@
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line11
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN GPIOB
diff --git a/hwconf/hw_49.h b/hwconf/hw_49.h
index ba2c36ee..1f7ccd02 100644
--- a/hwconf/hw_49.h
+++ b/hwconf/hw_49.h
@@ -148,6 +148,8 @@
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line11
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN GPIOB
diff --git a/hwconf/hw_r2.h b/hwconf/hw_r2.h
index a688ba24..68824f91 100644
--- a/hwconf/hw_r2.h
+++ b/hwconf/hw_r2.h
@@ -156,6 +156,8 @@
#define HW_ENC_EXTI_CH EXTI9_5_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line8
#define HW_ENC_EXTI_ISR_VEC EXTI9_5_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM3_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM3_IRQHandler
// NRF pins
#define NRF_PORT_CSN HW_ICU_GPIO
diff --git a/hwconf/hw_victor_r1a.h b/hwconf/hw_victor_r1a.h
index f0d1d174..943b4536 100644
--- a/hwconf/hw_victor_r1a.h
+++ b/hwconf/hw_victor_r1a.h
@@ -146,6 +146,8 @@
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line11
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN HW_ICU_GPIO
diff --git a/irq_handlers.c b/irq_handlers.c
index 9bc482fa..17cb9fe4 100644
--- a/irq_handlers.c
+++ b/irq_handlers.c
@@ -48,3 +48,12 @@ CH_IRQ_HANDLER(HW_ENC_EXTI_ISR_VEC) {
EXTI_ClearITPendingBit(HW_ENC_EXTI_LINE);
}
}
+
+CH_IRQ_HANDLER(HW_ENC_TIM_ISR_VEC) {
+ if (TIM_GetITStatus(HW_ENC_TIM, TIM_IT_Update) != RESET) {
+ encoder_tim_isr();
+
+ // Clear the IT pending bit
+ TIM_ClearITPendingBit(HW_ENC_TIM, TIM_IT_Update);
+ }
+}
diff --git a/main.c b/main.c
index 34075a79..caea5127 100644
--- a/main.c
+++ b/main.c
@@ -167,11 +167,6 @@ int main(void) {
mc_configuration mcconf;
conf_general_read_mc_configuration(&mcconf);
-
-#if ENCODER_ENABLE
- encoder_init(mcconf.m_encoder_counts);
-#endif
-
mc_interface_init(&mcconf);
commands_init();
@@ -208,8 +203,8 @@ int main(void) {
for(;;) {
chThdSleepMilliseconds(10);
-#if ENCODER_ENABLE
-// comm_can_set_pos(0, encoder_read_deg());
-#endif
+ if (encoder_is_configured()) {
+ // comm_can_set_pos(0, encoder_read_deg());
+ }
}
}
diff --git a/mc_interface.c b/mc_interface.c
index 89b47cdc..21b298b1 100644
--- a/mc_interface.c
+++ b/mc_interface.c
@@ -118,6 +118,22 @@ void mc_interface_init(mc_configuration *configuration) {
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
chThdCreateStatic(sample_send_thread_wa, sizeof(sample_send_thread_wa), NORMALPRIO - 1, sample_send_thread, NULL);
+ // Initialize encoder
+#if !WS2811_ENABLE
+ switch (m_conf.m_sensor_port_mode) {
+ case SENSOR_PORT_MODE_ABI:
+ encoder_init_abi(m_conf.m_encoder_counts);
+ break;
+
+ case SENSOR_PORT_MODE_AS5047_SPI:
+ encoder_init_as5047p_spi();
+ break;
+
+ default:
+ break;
+ }
+#endif
+
// Initialize selected implementation
switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
@@ -139,6 +155,28 @@ const volatile mc_configuration* mc_interface_get_configuration(void) {
}
void mc_interface_set_configuration(mc_configuration *configuration) {
+#if !WS2811_ENABLE
+ if (m_conf.m_sensor_port_mode != configuration->m_sensor_port_mode) {
+ encoder_deinit();
+ switch (configuration->m_sensor_port_mode) {
+ case SENSOR_PORT_MODE_ABI:
+ encoder_init_abi(configuration->m_encoder_counts);
+ break;
+
+ case SENSOR_PORT_MODE_AS5047_SPI:
+ encoder_init_as5047p_spi();
+ break;
+
+ default:
+ break;
+ }
+ }
+
+ if (configuration->m_sensor_port_mode == SENSOR_PORT_MODE_ABI) {
+ encoder_set_counts(configuration->m_encoder_counts);
+ }
+#endif
+
if (m_conf.motor_type == MOTOR_TYPE_FOC
&& configuration->motor_type != MOTOR_TYPE_FOC) {
mcpwm_foc_deinit();
@@ -432,7 +470,7 @@ float mc_interface_get_duty_cycle_now(void) {
return ret;
}
-float mc_interface_get_switching_frequency_now(void) {
+float mc_interface_get_sampling_frequency_now(void) {
float ret = 0.0;
switch (m_conf.motor_type) {
@@ -442,7 +480,7 @@ float mc_interface_get_switching_frequency_now(void) {
break;
case MOTOR_TYPE_FOC:
- ret = mcpwm_foc_get_switching_frequency_now();
+ ret = mcpwm_foc_get_switching_frequency_now() / 2.0;
break;
default:
@@ -918,14 +956,14 @@ void mc_interface_mc_timer_isr(void) {
}
// Watt and ah counters
- const float f_sw = mc_interface_get_switching_frequency_now();
+ const float f_samp = mc_interface_get_sampling_frequency_now();
if (fabsf(current) > 1.0) {
// Some extra filtering
static float curr_diff_sum = 0.0;
static float curr_diff_samples = 0;
- curr_diff_sum += current_in / f_sw;
- curr_diff_samples += 1.0 / f_sw;
+ curr_diff_sum += current_in / f_samp;
+ curr_diff_samples += 1.0 / f_samp;
if (curr_diff_samples >= 0.01) {
if (curr_diff_sum > 0.0) {
@@ -974,7 +1012,7 @@ void mc_interface_mc_timer_isr(void) {
m_vzero_samples[m_sample_now] = mcpwm_vzero;
m_curr_fir_samples[m_sample_now] = (int16_t)(mc_interface_get_tot_current() * 100.0);
- m_f_sw_samples[m_sample_now] = (int16_t)(mc_interface_get_switching_frequency_now() / 10.0);
+ m_f_sw_samples[m_sample_now] = (int16_t)(f_samp / 10.0);
m_status_samples[m_sample_now] = mcpwm_get_comm_step() | (mcpwm_read_hall_phase() << 3);
@@ -1016,7 +1054,7 @@ void mc_interface_adc_inj_int_handler(void) {
* The configaration to update.
*/
static void update_override_limits(volatile mc_configuration *conf) {
- const float temp = NTC_TEMP(ADC_IND_TEMP_MOS1);
+ const float temp = NTC_TEMP(ADC_IND_TEMP_MOS2);
const float v_in = GET_INPUT_VOLTAGE();
// Temperature
diff --git a/mc_interface.h b/mc_interface.h
index 18b24098..816ec2e0 100644
--- a/mc_interface.h
+++ b/mc_interface.h
@@ -48,7 +48,7 @@ void mc_interface_brake_now(void);
void mc_interface_release_motor(void);
float mc_interface_get_duty_cycle_set(void);
float mc_interface_get_duty_cycle_now(void);
-float mc_interface_get_switching_frequency_now(void);
+float mc_interface_get_sampling_frequency_now(void);
float mc_interface_get_rpm(void);
float mc_interface_get_amp_hours(bool reset);
float mc_interface_get_amp_hours_charged(bool reset);
diff --git a/mcconf/mcconf_default.h b/mcconf/mcconf_default.h
index 22695afc..0eb69869 100644
--- a/mcconf/mcconf_default.h
+++ b/mcconf/mcconf_default.h
@@ -279,8 +279,8 @@
#ifndef MCCONF_FOC_HALL_TAB_7
#define MCCONF_FOC_HALL_TAB_7 255
#endif
-#ifndef MCCONF_FOC_HALL_ERPM
-#define MCCONF_FOC_HALL_ERPM 2000.0 // ERPM above which only the observer is used
+#ifndef MCCONF_FOC_SL_ERPM
+#define MCCONF_FOC_SL_ERPM 2500.0 // ERPM above which only the observer is used
#endif
// Misc
@@ -299,5 +299,8 @@
#ifndef MCCONF_M_ENCODER_COUNTS
#define MCCONF_M_ENCODER_COUNTS 8192 // The number of encoder counts
#endif
+#ifndef MCCONF_M_SENSOR_PORT_MODE
+#define MCCONF_M_SENSOR_PORT_MODE SENSOR_PORT_MODE_HALL // The mode of the hall_encoder port
+#endif
#endif /* MCCONF_DEFAULT_H_ */
diff --git a/mcconf/mcconf_sten.h b/mcconf/mcconf_sten.h
index da1ac9db..31558485 100644
--- a/mcconf/mcconf_sten.h
+++ b/mcconf/mcconf_sten.h
@@ -25,44 +25,55 @@
#ifndef MCCONF_STEN_H_
#define MCCONF_STEN_H_
+/*
+ * HW: 4.10
+ */
+
/*
* Parameters
*/
+#define MCCONF_DEFAULT_MOTOR_TYPE MOTOR_TYPE_BLDC
#define MCCONF_L_CURRENT_MAX 35.0 // Current limit in Amperes (Upper)
#define MCCONF_L_CURRENT_MIN -30.0 // Current limit in Amperes (Lower)
-#define MCCONF_L_MAX_ABS_CURRENT 100.0 // The maximum absolute current above which a fault is generated
+#define MCCONF_L_MAX_ABS_CURRENT 130.0 // The maximum absolute current above which a fault is generated
#define MCCONF_L_SLOW_ABS_OVERCURRENT 1 // Use the filtered (and hence slower) current for the overcurrent fault detection
-#define MCCONF_L_IN_CURRENT_MAX 25.0 // Input current limit in Amperes (Upper)
-#define MCCONF_L_IN_CURRENT_MIN -20.0 // Input current limit in Amperes (Lower)
+#define MCCONF_L_IN_CURRENT_MAX 25.0 // Input current limit in Amperes (Upper)
+#define MCCONF_L_IN_CURRENT_MIN -20.0 // Input current limit in Amperes (Lower)
#define MCCONF_L_RPM_MAX 80000.0 // The motor speed limit (Upper)
#define MCCONF_L_RPM_MIN -80000.0 // The motor speed limit (Lower)
-#define MCCONF_L_MIN_VOLTAGE 20.0 // Minimum input voltage
-#define MCCONF_L_MAX_VOLTAGE 50.0 // Maximum input voltage
-#define MCCONF_CC_STARTUP_BOOST_DUTY 0.02 // The lowest duty cycle to use in current control mode (has to be > MCPWM_MIN_DUTY_CYCLE)
+#define MCCONF_L_MIN_VOLTAGE 10.0 // Minimum input voltage
+#define MCCONF_L_MAX_VOLTAGE 57.0 // Maximum input voltage
+#define MCCONF_CC_STARTUP_BOOST_DUTY 0.03 // The lowest duty cycle to use in current control mode (has to be > MCPWM_MIN_DUTY_CYCLE)
#define MCCONF_L_RPM_LIMIT_NEG_TORQUE 0 // Use negative torque to limit the RPM
#define MCCONF_L_CURR_MAX_RPM_FBRAKE 1500 // Maximum electrical RPM to use full brake at
+#define MCCONF_L_BATTERY_CUT_START 41.0 // Start limiting the positive current at this voltage
+#define MCCONF_L_BATTERY_CUT_END 39.0 // Limit the positive current completely at this voltage
+
// Sensorless settings
#define MCCONF_SENSOR_MODE SENSOR_MODE_SENSORLESS // Sensor mode
#define MCCONF_SL_MIN_RPM 250 // Auto-commutate below this RPM
-#define MCCONF_SL_MIN_ERPM_CYCLE_INT_LIMIT 500.0 // Minimum RPM to calculate the BEMF coupling from
+#define MCCONF_SL_MIN_ERPM_CYCLE_INT_LIMIT 1100.0 // Minimum RPM to calculate the BEMF coupling from
#define MCCONF_SL_CYCLE_INT_LIMIT 80.0 // Flux integrator limit 0 ERPM
#define MCCONF_SL_PHASE_ADVANCE_AT_BR 0.8 // Flux integrator limit percentage at MCPWM_CYCLE_INT_START_RPM_BR ERPM
#define MCCONF_SL_BEMF_COUPLING_K 750.0 // Input voltage to bemf coupling constant
+// FOC settings
+#define MCCONF_FOC_CURRENT_KP 0.03
+#define MCCONF_FOC_CURRENT_KI 50.0
+#define MCCONF_FOC_F_SW 20000.0
+#define MCCONF_FOC_MOTOR_L 0.000007
+#define MCCONF_FOC_MOTOR_R 0.015
+#define MCCONF_FOC_MOTOR_FLUX_LINKAGE 0.00245
+#define MCCONF_FOC_OBSERVER_GAIN 9e7
+#define MCCONF_FOC_OPENLOOP_RPM 600.0
+#define MCCONF_FOC_SL_OPENLOOP_HYST 0.5
+#define MCCONF_FOC_SL_OPENLOOP_TIME 0.5
+
// Speed PID parameters
#define MCCONF_S_PID_KP 0.0001 // Proportional gain
#define MCCONF_S_PID_KI 0.002 // Integral gain
#define MCCONF_S_PID_KD 0.0 // Derivative gain
#define MCCONF_S_PID_MIN_RPM 1200.0 // Minimum allowed RPM
-// Position PID parameters
-#define MCCONF_P_PID_KP 0.0001 // Proportional gain
-#define MCCONF_P_PID_KI 0.002 // Integral gain
-#define MCCONF_P_PID_KD 0.0 // Derivative gain
-
-// Current control parameters
-#define MCCONF_CC_GAIN 0.0046 // Current controller error gain
-#define MCCONF_CC_MIN_CURRENT 0.05 // Minimum allowed current
-
#endif /* MCCONF_STEN_H_ */
diff --git a/mcpwm.c b/mcpwm.c
index 396c5006..3a8df306 100644
--- a/mcpwm.c
+++ b/mcpwm.c
@@ -1916,7 +1916,7 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags) {
mc_interface_mc_timer_isr();
- if (ENCODER_ENABLE) {
+ if (encoder_is_configured()) {
run_pid_control_pos(1.0 / switching_frequency_now);
}
@@ -2045,7 +2045,7 @@ void mcpwm_reset_hall_detect_table(void) {
int mcpwm_get_hall_detect_result(int8_t *table) {
if (WS2811_ENABLE) {
return -2;
- } else if (ENCODER_ENABLE) {
+ } else if (conf->m_sensor_port_mode != SENSOR_PORT_MODE_HALL) {
return -3;
}
diff --git a/mcpwm_foc.c b/mcpwm_foc.c
index 5c80ccb9..9448b6ed 100644
--- a/mcpwm_foc.c
+++ b/mcpwm_foc.c
@@ -120,6 +120,7 @@ static void run_pid_control_speed(float dt);
static void stop_pwm_hw(void);
static void start_pwm_hw(void);
static int read_hall(void);
+static float correct_encoder(float obs_angle, float enc_angle, float speed);
static float correct_hall(float angle, float speed, float dt);
// Threads
@@ -829,7 +830,7 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
// Disable timeout
systime_t tout = timeout_get_timeout_msec();
float tout_c = timeout_get_brake_current();
- timeout_configure(60000, 0.0);
+ timeout_configure(600000, 0.0);
// Save configuration
float offset_old = m_conf->foc_encoder_offset;
@@ -872,34 +873,68 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
// Inverted and ratio
chThdSleepMilliseconds(1000);
+ const int it_rat = 20;
float s_sum = 0.0;
float c_sum = 0.0;
- for (int i = 0; i < 10; i++) {
+ float first = m_phase_now_encoder;
+
+ for (int i = 0; i < it_rat; i++) {
float phase_old = m_phase_now_encoder;
float phase_ovr_tmp = m_phase_now_override;
- for (float i = phase_ovr_tmp; i < phase_ovr_tmp + 0.8 * M_PI;
+ for (float i = phase_ovr_tmp; i < phase_ovr_tmp + (2.0 / 3.0) * M_PI;
i += (2.0 * M_PI) / 500.0) {
m_phase_now_override = i;
- chThdSleepMilliseconds(2);
+ chThdSleepMilliseconds(1);
}
- chThdSleepMilliseconds(1000);
- float diff = utils_angle_difference(
- m_phase_now_encoder * (180.0 / M_PI),
- phase_old * (180.0 / M_PI));
+ utils_norm_angle_rad((float*)&m_phase_now_override);
+ chThdSleepMilliseconds(300);
+ float diff = utils_angle_difference_rad(m_phase_now_encoder, phase_old);
float s, c;
- sincosf(diff * M_PI / 180.0, &s, &c);
+ sincosf(diff, &s, &c);
s_sum += s;
c_sum += c;
if (print) {
- commands_printf("%.2f", (double)diff);
+ commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
+ }
+
+ if (i > 3 && fabsf(utils_angle_difference_rad(m_phase_now_encoder, first)) < fabsf(diff / 2.0)) {
+ break;
+ }
+ }
+
+ first = m_phase_now_encoder;
+
+ for (int i = 0; i < it_rat; i++) {
+ float phase_old = m_phase_now_encoder;
+ float phase_ovr_tmp = m_phase_now_override;
+ for (float i = phase_ovr_tmp; i > phase_ovr_tmp - (2.0 / 3.0) * M_PI;
+ i -= (2.0 * M_PI) / 500.0) {
+ m_phase_now_override = i;
+ chThdSleepMilliseconds(1);
+ }
+ utils_norm_angle_rad((float*)&m_phase_now_override);
+ chThdSleepMilliseconds(300);
+ float diff = utils_angle_difference_rad(phase_old, m_phase_now_encoder);
+
+ float s, c;
+ sincosf(diff, &s, &c);
+ s_sum += s;
+ c_sum += c;
+
+ if (print) {
+ commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
+ }
+
+ if (i > 3 && fabsf(utils_angle_difference_rad(m_phase_now_encoder, first)) < fabsf(diff / 2.0)) {
+ break;
}
}
float diff = atan2f(s_sum, c_sum) * 180.0 / M_PI;
*inverted = diff < 0.0;
- *ratio = roundf((0.8 * 180.0) /
+ *ratio = roundf(((2.0 / 3.0) * 180.0) /
fabsf(diff));
m_conf->foc_encoder_inverted = *inverted;
@@ -920,33 +955,37 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
commands_printf("Enc: %.2f", (double)encoder_read_deg());
}
+ const int it_ofs = m_conf->foc_encoder_ratio * 3.0;
s_sum = 0.0;
c_sum = 0.0;
- for (int i = 0;i < 5;i++) {
- m_phase_now_override = (float)i * M_PI / 5.0;
- chThdSleepMilliseconds(1000);
+ for (int i = 0;i < it_ofs;i++) {
+ m_phase_now_override = ((float)i * 2.0 * M_PI * m_conf->foc_encoder_ratio) / ((float)it_ofs);
+ chThdSleepMilliseconds(500);
+
+ float diff = utils_angle_difference_rad(m_phase_now_encoder, m_phase_now_override);
float s, c;
- sincosf(m_phase_now_encoder - m_phase_now_override, &s, &c);
+ sincosf(diff, &s, &c);
s_sum += s;
c_sum += c;
if (print) {
- commands_printf("%.2f", (double)((m_phase_now_encoder - m_phase_now_override) * 180.0 / M_PI));
+ commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
}
}
- for (int i = 3;i >= -1;i--) {
- m_phase_now_override = (float)i * M_PI / 5.0;
- chThdSleepMilliseconds(1000);
+ for (int i = it_ofs;i > 0;i--) {
+ m_phase_now_override = ((float)i * 2.0 * M_PI * m_conf->foc_encoder_ratio) / ((float)it_ofs);
+ chThdSleepMilliseconds(500);
+ float diff = utils_angle_difference_rad(m_phase_now_encoder, m_phase_now_override);
float s, c;
- sincosf(m_phase_now_encoder - m_phase_now_override, &s, &c);
+ sincosf(diff, &s, &c);
s_sum += s;
c_sum += c;
if (print) {
- commands_printf("%.2f", (double)((m_phase_now_encoder - m_phase_now_override) * 180.0 / M_PI));
+ commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
}
}
@@ -1370,16 +1409,18 @@ void mcpwm_foc_adc_inj_int_handler(void) {
m_motor_state.v_bus = GET_INPUT_VOLTAGE();
-#if ENCODER_ENABLE
- float phase_tmp = encoder_read_deg();
- if (m_conf->foc_encoder_inverted) {
- phase_tmp = 360.0 - phase_tmp;
+ float enc_ang = 0;
+ if (encoder_is_configured()) {
+ enc_ang = encoder_read_deg();
+ float phase_tmp = enc_ang;
+ if (m_conf->foc_encoder_inverted) {
+ phase_tmp = 360.0 - phase_tmp;
+ }
+ phase_tmp *= m_conf->foc_encoder_ratio;
+ phase_tmp -= m_conf->foc_encoder_offset;
+ utils_norm_angle((float*)&phase_tmp);
+ m_phase_now_encoder = phase_tmp * (M_PI / 180.0);
}
- phase_tmp *= m_conf->foc_encoder_ratio;
- phase_tmp -= m_conf->foc_encoder_offset;
- utils_norm_angle((float*)&phase_tmp);
- m_phase_now_encoder = phase_tmp * (M_PI / 180.0);
-#endif
static float phase_before = 0.0;
const float phase_diff = utils_angle_difference_rad(m_motor_state.phase, phase_before);
@@ -1410,7 +1451,7 @@ void mcpwm_foc_adc_inj_int_handler(void) {
// observer has lost tracking. Use duty cycle control with the lowest duty cycle
// to get as smooth braking as possible.
if (m_control_mode == CONTROL_MODE_CURRENT_BRAKE
- && (m_conf->foc_sensor_mode != FOC_SENSOR_MODE_ENCODER)
+// && (m_conf->foc_sensor_mode != FOC_SENSOR_MODE_ENCODER) // Don't use this with encoderss
&& fabsf(duty_filtered) < 0.03) {
control_duty = true;
duty_set = 0.0;
@@ -1479,7 +1520,7 @@ void mcpwm_foc_adc_inj_int_handler(void) {
switch (m_conf->foc_sensor_mode) {
case FOC_SENSOR_MODE_ENCODER:
if (encoder_index_found()) {
- m_motor_state.phase = m_phase_now_encoder;
+ m_motor_state.phase = correct_encoder(m_phase_now_observer, m_phase_now_encoder, m_pll_speed);
} else {
// Rotate the motor in open loop if the index isn't found.
m_motor_state.phase = m_phase_now_encoder_no_index;
@@ -1492,6 +1533,10 @@ void mcpwm_foc_adc_inj_int_handler(void) {
case FOC_SENSOR_MODE_HALL:
m_phase_now_observer = correct_hall(m_phase_now_observer, m_pll_speed, dt);
m_motor_state.phase = m_phase_now_observer;
+
+ if (!m_phase_override) {
+ id_set_tmp = 0.0;
+ }
break;
case FOC_SENSOR_MODE_SENSORLESS:
if (m_phase_observer_override) {
@@ -1571,7 +1616,7 @@ void mcpwm_foc_adc_inj_int_handler(void) {
switch (m_conf->foc_sensor_mode) {
case FOC_SENSOR_MODE_ENCODER:
- m_motor_state.phase = m_phase_now_encoder;
+ m_motor_state.phase = correct_encoder(m_phase_now_observer, m_phase_now_encoder, m_pll_speed);
break;
case FOC_SENSOR_MODE_HALL:
m_phase_now_observer = correct_hall(m_phase_now_observer, m_pll_speed, dt);
@@ -1602,11 +1647,12 @@ void mcpwm_foc_adc_inj_int_handler(void) {
// Track position control angle
// TODO: Have another look at this.
-#if ENCODER_ENABLE
- float angle_now = encoder_read_deg();
-#else
- float angle_now = m_motor_state.phase * (180.0 / M_PI);
-#endif
+ float angle_now = 0.0;
+ if (encoder_is_configured()) {
+ angle_now = enc_ang;
+ } else {
+ angle_now = m_motor_state.phase * (180.0 / M_PI);
+ }
if (m_conf->p_pid_ang_div > 0.98 && m_conf->p_pid_ang_div < 1.02) {
m_pos_pid_now = angle_now;
@@ -2005,11 +2051,11 @@ static void run_pid_control_pos(float angle_now, float angle_set, float dt) {
// Compute parameters
float error = utils_angle_difference(angle_set, angle_now);
-#if ENCODER_ENABLE
- if (m_conf->foc_encoder_inverted) {
- error = -error;
+ if (encoder_is_configured()) {
+ if (m_conf->foc_encoder_inverted) {
+ error = -error;
+ }
}
-#endif
p_term = error * m_conf->p_pid_kp;
i_term += error * (m_conf->p_pid_ki * dt);
@@ -2037,16 +2083,16 @@ static void run_pid_control_pos(float angle_now, float angle_set, float dt) {
float output = p_term + i_term + d_term;
utils_truncate_number(&output, -1.0, 1.0);
-#if ENCODER_ENABLE
- if (encoder_index_found()) {
- m_iq_set = output * m_conf->lo_current_max;
+ if (encoder_is_configured()) {
+ if (encoder_index_found()) {
+ m_iq_set = output * m_conf->lo_current_max;
+ } else {
+ // Rotate the motor with 40 % power until the encoder index is found.
+ m_iq_set = 0.4 * m_conf->lo_current_max;
+ }
} else {
- // Rotate the motor with 40 % power until the encoder index is found.
- m_iq_set = 0.4 * m_conf->lo_current_max;
+ m_iq_set = output * m_conf->lo_current_max;
}
-#else
- m_iq_set = output * m_conf->lo_current_max;
-#endif
}
static void run_pid_control_speed(float dt) {
@@ -2133,18 +2179,38 @@ static int read_hall(void) {
return READ_HALL1() | (READ_HALL2() << 1) | (READ_HALL3() << 2);
}
+static float correct_encoder(float obs_angle, float enc_angle, float speed) {
+ float rpm_abs = fabsf(speed / ((2.0 * M_PI) / 60.0));
+ static bool using_encoder = true;
+
+ // Hysteresis 5 % of total speed
+ float hyst = m_conf->foc_sl_erpm * 0.05;
+ if (using_encoder) {
+ if (rpm_abs > (m_conf->foc_sl_erpm + hyst)) {
+ using_encoder = false;
+ }
+ } else {
+ if (rpm_abs < (m_conf->foc_sl_erpm - hyst)) {
+ using_encoder = true;
+ }
+ }
+
+ return using_encoder ? enc_angle : obs_angle;
+}
+
static float correct_hall(float angle, float speed, float dt) {
static int ang_hall_int_prev = -1;
float rpm_abs = fabsf(speed / ((2.0 * M_PI) / 60.0));
static bool using_hall = true;
- // Hysteresis of 100 rpm
+ // Hysteresis 5 % of total speed
+ float hyst = m_conf->foc_sl_erpm * 0.05;
if (using_hall) {
- if (rpm_abs > (m_conf->foc_hall_sl_erpm + 100)) {
+ if (rpm_abs > (m_conf->foc_sl_erpm + hyst)) {
using_hall = false;
}
} else {
- if (rpm_abs < (m_conf->foc_hall_sl_erpm - 100)) {
+ if (rpm_abs < (m_conf->foc_sl_erpm - hyst)) {
using_hall = true;
}
}
diff --git a/mcuconf.h b/mcuconf.h
index dbf728c9..e9de5f29 100644
--- a/mcuconf.h
+++ b/mcuconf.h
@@ -248,7 +248,7 @@
/*
* SPI driver system settings.
*/
-#define STM32_SPI_USE_SPI1 FALSE
+#define STM32_SPI_USE_SPI1 TRUE
#define STM32_SPI_USE_SPI2 FALSE
#define STM32_SPI_USE_SPI3 FALSE
#define STM32_SPI_SPI1_RX_DMA_STREAM STM32_DMA_STREAM_ID(2, 0)
diff --git a/terminal.c b/terminal.c
index fe2acece..0315005e 100644
--- a/terminal.c
+++ b/terminal.c
@@ -33,6 +33,7 @@
#include "comm_can.h"
#include "utils.h"
#include "timeout.h"
+#include "encoder.h"
#include
#include
@@ -221,25 +222,25 @@ void terminal_process_string(char *str) {
sscanf(argv[1], "%f", ¤t);
if (current > 0.0 && current <= mcconf.l_current_max) {
-#if ENCODER_ENABLE
- mc_motor_type type_old = mcconf.motor_type;
- mcconf.motor_type = MOTOR_TYPE_FOC;
- mc_interface_set_configuration(&mcconf);
+ if (encoder_is_configured()) {
+ mc_motor_type type_old = mcconf.motor_type;
+ mcconf.motor_type = MOTOR_TYPE_FOC;
+ mc_interface_set_configuration(&mcconf);
- float offset = 0.0;
- float ratio = 0.0;
- bool inverted = false;
- mcpwm_foc_encoder_detect(current, true, &offset, &ratio, &inverted);
+ float offset = 0.0;
+ float ratio = 0.0;
+ bool inverted = false;
+ mcpwm_foc_encoder_detect(current, true, &offset, &ratio, &inverted);
- mcconf.motor_type = type_old;
- mc_interface_set_configuration(&mcconf);
+ mcconf.motor_type = type_old;
+ mc_interface_set_configuration(&mcconf);
- commands_printf("Offset : %.2f", (double)offset);
- commands_printf("Ratio : %.2f", (double)ratio);
- commands_printf("Inverted : %s\n", inverted ? "true" : "false");
-#else
- commands_printf("Encoder not enabled.\n");
-#endif
+ commands_printf("Offset : %.2f", (double)offset);
+ commands_printf("Ratio : %.2f", (double)ratio);
+ commands_printf("Inverted : %s\n", inverted ? "true" : "false");
+ } else {
+ commands_printf("Encoder not enabled.\n");
+ }
} else {
commands_printf("Invalid argument(s).\n");
}