diff --git a/CHANGELOG b/CHANGELOG
index 906b4e6c5..9d78a24d9 100644
--- a/CHANGELOG
+++ b/CHANGELOG
@@ -1,3 +1,19 @@
+=== FW 3.66 ===
+* Added support for HW 100/250.
+* Added uptime terminal command.
+* Added some delays to DRV8323s SPI driver.
+* Added SWD support for NRF52840 with idcode 0x015B.
+ - TODO: Have a look at https://github.com/blacksphere/blackmagic/commit/302ff20a6d5b806c09e0ca7e996beab3ef3596f4.
+* Fixed INVERTED_SHUNT_POLARITY for BLDC.
+* Added decoupling to FOC current controller.
+* Better motor tracking at high ERPM and low Fsw.
+* Made uart and permanent uart more independent.
+* Do not write to USB if cable has not been connected.
+* Added timeout to USB write.
+* Better FOC current control integrator windup protection.
+* Added FOC observer type selection options.
+* Print TS5700N8501 position in encoder terminal command.
+
=== FW 3.65 ===
* Added support for PTC motor temperature sensor (e.g. KTY84)
* APP_PPM sleep fix. Should solve CAN issues.
diff --git a/applications/app.h b/applications/app.h
index 4bb02f439..1e81ea411 100644
--- a/applications/app.h
+++ b/applications/app.h
@@ -47,6 +47,7 @@ void app_uartcomm_start_permanent(void);
void app_uartcomm_stop(void);
void app_uartcomm_configure(uint32_t baudrate, bool permanent_enabled);
void app_uartcomm_send_packet(unsigned char *data, unsigned int len);
+void app_uartcomm_send_packet_p(unsigned char *data, unsigned int len);
void app_nunchuk_start(void);
void app_nunchuk_stop(void);
diff --git a/applications/app_uartcomm.c b/applications/app_uartcomm.c
index fb9f7e6ae..0d0fed354 100644
--- a/applications/app_uartcomm.c
+++ b/applications/app_uartcomm.c
@@ -41,10 +41,20 @@ static volatile bool uart_is_running = false;
static mutex_t send_mutex;
static bool send_mutex_init_done = false;
+#ifdef HW_UART_P_DEV
+static mutex_t send_mutex_p;
+static bool send_mutex_p_init_done = false;
+#endif
+
// Private functions
static void process_packet(unsigned char *data, unsigned int len);
static void send_packet(unsigned char *data, unsigned int len);
+#ifdef HW_UART_P_DEV
+static void process_packet_p(unsigned char *data, unsigned int len);
+static void send_packet_p(unsigned char *data, unsigned int len);
+#endif
+
static SerialConfig uart_cfg = {
BAUDRATE,
0,
@@ -67,27 +77,29 @@ static void process_packet(unsigned char *data, unsigned int len) {
commands_process_packet(data, len, app_uartcomm_send_packet);
}
-static void send_packet(unsigned char *data, unsigned int len) {
#ifdef HW_UART_P_DEV
- if (from_p_uart) {
- if (uart_p_is_running) {
-#ifdef HW_UART_P_DEV_TX
- sdWrite(&HW_UART_P_DEV_TX, data, len);
-#else
- sdWrite(&HW_UART_P_DEV, data, len);
+static void process_packet_p(unsigned char *data, unsigned int len) {
+ commands_process_packet(data, len, app_uartcomm_send_packet_p);
+}
#endif
- }
- } else {
- if (uart_is_running) {
- sdWrite(&HW_UART_DEV, data, len);
- }
- }
-#else
+
+static void send_packet(unsigned char *data, unsigned int len) {
if (uart_is_running) {
sdWrite(&HW_UART_DEV, data, len);
}
+}
+
+#ifdef HW_UART_P_DEV
+static void send_packet_p(unsigned char *data, unsigned int len) {
+ if (uart_p_is_running) {
+#ifdef HW_UART_P_DEV_TX
+ sdWrite(&HW_UART_P_DEV_TX, data, len);
+#else
+ sdWrite(&HW_UART_P_DEV, data, len);
#endif
+ }
}
+#endif
void app_uartcomm_start(void) {
packet_init(send_packet, process_packet, PACKET_HANDLER);
@@ -111,8 +123,7 @@ void app_uartcomm_start(void) {
void app_uartcomm_start_permanent(void) {
#ifdef HW_UART_P_DEV
- packet_init(send_packet, process_packet, PACKET_HANDLER);
- packet_init(send_packet, process_packet, PACKET_HANDLER_P);
+ packet_init(send_packet_p, process_packet_p, PACKET_HANDLER_P);
if (!thread_is_running) {
chThdCreateStatic(packet_process_thread_wa, sizeof(packet_process_thread_wa),
@@ -159,6 +170,22 @@ void app_uartcomm_send_packet(unsigned char *data, unsigned int len) {
chMtxUnlock(&send_mutex);
}
+void app_uartcomm_send_packet_p(unsigned char *data, unsigned int len) {
+#ifdef HW_UART_P_DEV
+ if (!send_mutex_p_init_done) {
+ chMtxObjectInit(&send_mutex_p);
+ send_mutex_p_init_done = true;
+ }
+
+ chMtxLock(&send_mutex_p);
+ packet_send_packet(data, len, PACKET_HANDLER_P);
+ chMtxUnlock(&send_mutex_p);
+#else
+ (void)data;
+ (void)len;
+#endif
+}
+
void app_uartcomm_configure(uint32_t baudrate, bool permanent_enabled) {
uart_cfg.speed = baudrate;
diff --git a/blackmagic/bm_if.c b/blackmagic/bm_if.c
index fcf1e2b96..d1963e9ef 100644
--- a/blackmagic/bm_if.c
+++ b/blackmagic/bm_if.c
@@ -133,6 +133,7 @@ static int idcode_to_device(uint32_t idcode) {
ret = 7; break;
case 0x00EB: /* nRF52840 Preview QIAA AA0 */
case 0x0150: /* nRF52840 QIAA C0 */
+ case 0x015B: /* nRF52840 ?? */
ret = 8; break;
default: ret = -2; break;
}
@@ -362,6 +363,9 @@ int bm_connect(void) {
if (cur_target) {
ret = idcode_to_device(target_idcode(cur_target));
+ if (ret < 0) {
+ commands_printf("Unknown idcode: 0x%04X\n", target_idcode(cur_target));
+ }
}
}
diff --git a/blackmagic/target/nrf51.c b/blackmagic/target/nrf51.c
index ba3434cf2..1dab820d0 100644
--- a/blackmagic/target/nrf51.c
+++ b/blackmagic/target/nrf51.c
@@ -176,6 +176,7 @@ bool nrf51_probe(target *t)
return true;
case 0x00EB: /* nRF52840 Preview QIAA AA0 */
case 0x0150: /* nRF52840 QIAA C0 */
+ case 0x015B: /* nRF52840 ?? */
t->driver = "Nordic nRF52";
target_add_ram(t, 0x20000000, 256*1024);
nrf51_add_flash(t, 0x00000000, 1024*1024, NRF52_PAGE_SIZE);
diff --git a/build_all/100_250/VESC_default.bin b/build_all/100_250/VESC_default.bin
new file mode 100755
index 000000000..5c57ca1a1
Binary files /dev/null and b/build_all/100_250/VESC_default.bin differ
diff --git a/build_all/100_250/VESC_default_no_hw_limits.bin b/build_all/100_250/VESC_default_no_hw_limits.bin
new file mode 100755
index 000000000..a385f07a9
Binary files /dev/null and b/build_all/100_250/VESC_default_no_hw_limits.bin differ
diff --git a/build_all/100_250/VESC_servoout.bin b/build_all/100_250/VESC_servoout.bin
new file mode 100755
index 000000000..bf5e3ab65
Binary files /dev/null and b/build_all/100_250/VESC_servoout.bin differ
diff --git a/build_all/410_o_411_o_412/VESC_0005ohm.bin b/build_all/410_o_411_o_412/VESC_0005ohm.bin
index 47563c4e0..85af2cd42 100755
Binary files a/build_all/410_o_411_o_412/VESC_0005ohm.bin and b/build_all/410_o_411_o_412/VESC_0005ohm.bin differ
diff --git a/build_all/410_o_411_o_412/VESC_default.bin b/build_all/410_o_411_o_412/VESC_default.bin
index 24d1b60bb..dc82b183e 100755
Binary files a/build_all/410_o_411_o_412/VESC_default.bin and b/build_all/410_o_411_o_412/VESC_default.bin differ
diff --git a/build_all/410_o_411_o_412/VESC_default_no_hw_limits.bin b/build_all/410_o_411_o_412/VESC_default_no_hw_limits.bin
index 5b0ca14aa..eb55b70d0 100755
Binary files a/build_all/410_o_411_o_412/VESC_default_no_hw_limits.bin and b/build_all/410_o_411_o_412/VESC_default_no_hw_limits.bin differ
diff --git a/build_all/410_o_411_o_412/VESC_servoout.bin b/build_all/410_o_411_o_412/VESC_servoout.bin
index 43837d7fe..8f2b70715 100755
Binary files a/build_all/410_o_411_o_412/VESC_servoout.bin and b/build_all/410_o_411_o_412/VESC_servoout.bin differ
diff --git a/build_all/410_o_411_o_412/VESC_ws2811.bin b/build_all/410_o_411_o_412/VESC_ws2811.bin
index 0c74c545a..7a211f979 100755
Binary files a/build_all/410_o_411_o_412/VESC_ws2811.bin and b/build_all/410_o_411_o_412/VESC_ws2811.bin differ
diff --git a/build_all/46_o_47/VESC_0005ohm.bin b/build_all/46_o_47/VESC_0005ohm.bin
index e68e3b932..4e8f67fc5 100755
Binary files a/build_all/46_o_47/VESC_0005ohm.bin and b/build_all/46_o_47/VESC_0005ohm.bin differ
diff --git a/build_all/46_o_47/VESC_33k.bin b/build_all/46_o_47/VESC_33k.bin
index b5f8f1579..4e6554600 100755
Binary files a/build_all/46_o_47/VESC_33k.bin and b/build_all/46_o_47/VESC_33k.bin differ
diff --git a/build_all/46_o_47/VESC_default.bin b/build_all/46_o_47/VESC_default.bin
index 1315b6cea..764c1888c 100755
Binary files a/build_all/46_o_47/VESC_default.bin and b/build_all/46_o_47/VESC_default.bin differ
diff --git a/build_all/46_o_47/VESC_servoout.bin b/build_all/46_o_47/VESC_servoout.bin
index 6265f770b..96a13ee74 100755
Binary files a/build_all/46_o_47/VESC_servoout.bin and b/build_all/46_o_47/VESC_servoout.bin differ
diff --git a/build_all/46_o_47/VESC_ws2811.bin b/build_all/46_o_47/VESC_ws2811.bin
index 8dd7b1a75..0e0ca58bd 100755
Binary files a/build_all/46_o_47/VESC_ws2811.bin and b/build_all/46_o_47/VESC_ws2811.bin differ
diff --git a/build_all/46_o_47/VESC_ws2811_33k.bin b/build_all/46_o_47/VESC_ws2811_33k.bin
index 848e33f9e..63acb4a5e 100755
Binary files a/build_all/46_o_47/VESC_ws2811_33k.bin and b/build_all/46_o_47/VESC_ws2811_33k.bin differ
diff --git a/build_all/48/VESC_0005ohm.bin b/build_all/48/VESC_0005ohm.bin
index e533890d6..940ffa261 100755
Binary files a/build_all/48/VESC_0005ohm.bin and b/build_all/48/VESC_0005ohm.bin differ
diff --git a/build_all/48/VESC_default.bin b/build_all/48/VESC_default.bin
index 703c398da..9e44e8142 100755
Binary files a/build_all/48/VESC_default.bin and b/build_all/48/VESC_default.bin differ
diff --git a/build_all/48/VESC_servoout.bin b/build_all/48/VESC_servoout.bin
index a2b91ab66..ef060382a 100755
Binary files a/build_all/48/VESC_servoout.bin and b/build_all/48/VESC_servoout.bin differ
diff --git a/build_all/48/VESC_ws2811.bin b/build_all/48/VESC_ws2811.bin
index 9d907c5ac..424b13521 100755
Binary files a/build_all/48/VESC_ws2811.bin and b/build_all/48/VESC_ws2811.bin differ
diff --git a/build_all/60/VESC_default.bin b/build_all/60/VESC_default.bin
index 97abd413e..4c19ec78a 100755
Binary files a/build_all/60/VESC_default.bin and b/build_all/60/VESC_default.bin differ
diff --git a/build_all/60/VESC_default_no_hw_limits.bin b/build_all/60/VESC_default_no_hw_limits.bin
index f8f31f64f..d4525156f 100755
Binary files a/build_all/60/VESC_default_no_hw_limits.bin and b/build_all/60/VESC_default_no_hw_limits.bin differ
diff --git a/build_all/60/VESC_servoout.bin b/build_all/60/VESC_servoout.bin
index 8637365d5..43d6f0c92 100755
Binary files a/build_all/60/VESC_servoout.bin and b/build_all/60/VESC_servoout.bin differ
diff --git a/build_all/60/VESC_ws2811.bin b/build_all/60/VESC_ws2811.bin
index e702e542a..63685db9e 100755
Binary files a/build_all/60/VESC_ws2811.bin and b/build_all/60/VESC_ws2811.bin differ
diff --git a/build_all/60_MK3/VESC_default.bin b/build_all/60_MK3/VESC_default.bin
index 8ef97f322..fbcfffbeb 100755
Binary files a/build_all/60_MK3/VESC_default.bin and b/build_all/60_MK3/VESC_default.bin differ
diff --git a/build_all/60_MK3/VESC_default_no_hw_limits.bin b/build_all/60_MK3/VESC_default_no_hw_limits.bin
index a9553a9e0..b1c9b3c0f 100755
Binary files a/build_all/60_MK3/VESC_default_no_hw_limits.bin and b/build_all/60_MK3/VESC_default_no_hw_limits.bin differ
diff --git a/build_all/60_MK3/VESC_servoout.bin b/build_all/60_MK3/VESC_servoout.bin
index 4372c54c9..0e6cbd55f 100755
Binary files a/build_all/60_MK3/VESC_servoout.bin and b/build_all/60_MK3/VESC_servoout.bin differ
diff --git a/build_all/60_MK3/VESC_ws2811.bin b/build_all/60_MK3/VESC_ws2811.bin
index 07bb4bfc2..1da7bdd08 100755
Binary files a/build_all/60_MK3/VESC_ws2811.bin and b/build_all/60_MK3/VESC_ws2811.bin differ
diff --git a/build_all/75_300/VESC_default.bin b/build_all/75_300/VESC_default.bin
index da8b48dda..0f220e4e5 100755
Binary files a/build_all/75_300/VESC_default.bin and b/build_all/75_300/VESC_default.bin differ
diff --git a/build_all/75_300/VESC_default_no_hw_limits.bin b/build_all/75_300/VESC_default_no_hw_limits.bin
index 93daec04c..c99ac1f5c 100755
Binary files a/build_all/75_300/VESC_default_no_hw_limits.bin and b/build_all/75_300/VESC_default_no_hw_limits.bin differ
diff --git a/build_all/75_300/VESC_servoout.bin b/build_all/75_300/VESC_servoout.bin
index 12059f938..c04218696 100755
Binary files a/build_all/75_300/VESC_servoout.bin and b/build_all/75_300/VESC_servoout.bin differ
diff --git a/build_all/75_300/VESC_ws2811.bin b/build_all/75_300/VESC_ws2811.bin
index a770cbc26..f1c8af777 100755
Binary files a/build_all/75_300/VESC_ws2811.bin and b/build_all/75_300/VESC_ws2811.bin differ
diff --git a/build_all/75_300_R2/VESC_default.bin b/build_all/75_300_R2/VESC_default.bin
index 3b01be60d..9d7e641bb 100755
Binary files a/build_all/75_300_R2/VESC_default.bin and b/build_all/75_300_R2/VESC_default.bin differ
diff --git a/build_all/75_300_R2/VESC_default_no_hw_limits.bin b/build_all/75_300_R2/VESC_default_no_hw_limits.bin
index 89b376904..053169164 100755
Binary files a/build_all/75_300_R2/VESC_default_no_hw_limits.bin and b/build_all/75_300_R2/VESC_default_no_hw_limits.bin differ
diff --git a/build_all/75_300_R2/VESC_servoout.bin b/build_all/75_300_R2/VESC_servoout.bin
index 2d0ac95a1..565cba691 100755
Binary files a/build_all/75_300_R2/VESC_servoout.bin and b/build_all/75_300_R2/VESC_servoout.bin differ
diff --git a/build_all/75_300_R2/VESC_ws2811.bin b/build_all/75_300_R2/VESC_ws2811.bin
index f60eba429..349487e70 100755
Binary files a/build_all/75_300_R2/VESC_ws2811.bin and b/build_all/75_300_R2/VESC_ws2811.bin differ
diff --git a/build_all/A200S_V21/VESC_default.bin b/build_all/A200S_V21/VESC_default.bin
index 914ac0af8..faea4a135 100755
Binary files a/build_all/A200S_V21/VESC_default.bin and b/build_all/A200S_V21/VESC_default.bin differ
diff --git a/build_all/A200S_V22/VESC_default.bin b/build_all/A200S_V22/VESC_default.bin
index b612bbfff..7df23f1d2 100755
Binary files a/build_all/A200S_V22/VESC_default.bin and b/build_all/A200S_V22/VESC_default.bin differ
diff --git a/build_all/AXIOM/VESC_default.bin b/build_all/AXIOM/VESC_default.bin
index fa5342feb..fa8c79864 100755
Binary files a/build_all/AXIOM/VESC_default.bin and b/build_all/AXIOM/VESC_default.bin differ
diff --git a/build_all/DAS_RS/VESC_default.bin b/build_all/DAS_RS/VESC_default.bin
index bd2c8cf87..da7d09e41 100755
Binary files a/build_all/DAS_RS/VESC_default.bin and b/build_all/DAS_RS/VESC_default.bin differ
diff --git a/build_all/HD/VESC_default.bin b/build_all/HD/VESC_default.bin
index 0a0b0347a..89c9c7541 100755
Binary files a/build_all/HD/VESC_default.bin and b/build_all/HD/VESC_default.bin differ
diff --git a/build_all/HD/VESC_default_no_hw_limits.bin b/build_all/HD/VESC_default_no_hw_limits.bin
index d4518289b..7abcea0b4 100755
Binary files a/build_all/HD/VESC_default_no_hw_limits.bin and b/build_all/HD/VESC_default_no_hw_limits.bin differ
diff --git a/build_all/HD/VESC_servoout.bin b/build_all/HD/VESC_servoout.bin
index 9f4db064c..cea9b870a 100755
Binary files a/build_all/HD/VESC_servoout.bin and b/build_all/HD/VESC_servoout.bin differ
diff --git a/build_all/HD/VESC_ws2811.bin b/build_all/HD/VESC_ws2811.bin
index b535d50d7..03f3f0fce 100755
Binary files a/build_all/HD/VESC_ws2811.bin and b/build_all/HD/VESC_ws2811.bin differ
diff --git a/build_all/UAVC_OMEGA/VESC_default.bin b/build_all/UAVC_OMEGA/VESC_default.bin
index f0cf4b023..a62c4b796 100755
Binary files a/build_all/UAVC_OMEGA/VESC_default.bin and b/build_all/UAVC_OMEGA/VESC_default.bin differ
diff --git a/build_all/rebuild_all b/build_all/rebuild_all
index 883d6e4f1..08078505f 100755
--- a/build_all/rebuild_all
+++ b/build_all/rebuild_all
@@ -399,3 +399,34 @@ cd $FWPATH
make clean
cd $DIR
+#################### HW 100_250 ########################
+
+COPYDIR=100_250
+rm -f $COPYDIR/*
+
+# default
+cd $FWPATH
+touch conf_general.h
+make -j8 build_args='-DHW_SOURCE=\"hw_100_250.c\" -DHW_HEADER=\"hw_100_250.h\"' USE_VERBOSE_COMPILE=no
+cd $DIR
+cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_default.bin
+
+# default with HW limits disables
+cd $FWPATH
+touch conf_general.h
+make -j8 build_args='-DDISABLE_HW_LIMITS -DHW_SOURCE=\"hw_100_250.c\" -DHW_HEADER=\"hw_100_250.h\"' USE_VERBOSE_COMPILE=no
+cd $DIR
+cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_default_no_hw_limits.bin
+
+# servoout
+cd $FWPATH
+touch conf_general.h
+make -j8 build_args='-DSERVO_OUT_ENABLE=1 -DHW_SOURCE=\"hw_100_250.c\" -DHW_HEADER=\"hw_100_250.h\"' USE_VERBOSE_COMPILE=no
+cd $DIR
+cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_servoout.bin
+
+# Clean
+cd $FWPATH
+make clean
+cd $DIR
+
diff --git a/comm_usb.c b/comm_usb.c
index 0935fa937..3944fb19c 100644
--- a/comm_usb.c
+++ b/comm_usb.c
@@ -32,10 +32,11 @@
static uint8_t serial_rx_buffer[SERIAL_RX_BUFFER_SIZE];
static int serial_rx_read_pos = 0;
static int serial_rx_write_pos = 0;
-static THD_WORKING_AREA(serial_read_thread_wa, 256);
+static THD_WORKING_AREA(serial_read_thread_wa, 128);
static THD_WORKING_AREA(serial_process_thread_wa, 4096);
static mutex_t send_mutex;
static thread_t *process_tp;
+static volatile unsigned int write_timeout_cnt = 0;
// Private functions
static void process_packet(unsigned char *data, unsigned int len);
@@ -44,9 +45,9 @@ static void send_packet_raw(unsigned char *buffer, unsigned int len);
static THD_FUNCTION(serial_read_thread, arg) {
(void)arg;
- chRegSetThreadName("USB-Serial read");
+ chRegSetThreadName("USB read");
- uint8_t buffer[128];
+ uint8_t buffer[2];
int had_data = 0;
for(;;) {
@@ -72,7 +73,7 @@ static THD_FUNCTION(serial_read_thread, arg) {
static THD_FUNCTION(serial_process_thread, arg) {
(void)arg;
- chRegSetThreadName("USB-Serial process");
+ chRegSetThreadName("USB process");
process_tp = chThdGetSelfX();
@@ -94,7 +95,26 @@ static void process_packet(unsigned char *data, unsigned int len) {
}
static void send_packet_raw(unsigned char *buffer, unsigned int len) {
- chSequentialStreamWrite(&SDU1, buffer, len);
+ static bool was_timeout = false;
+
+ /*
+ * Only write to USB if the cable has been connected at least once. If a timeout occurs
+ * make sure that this call does not stall on the next call, as the timeout probably occured
+ * because noone is listening on the USB.
+ */
+ if (comm_usb_serial_configured_cnt() > 0) {
+ unsigned int written = 0;
+ if (was_timeout) {
+ written = SDU1.vmt->writet(&SDU1, buffer, len, TIME_IMMEDIATE);
+ } else {
+ written = SDU1.vmt->writet(&SDU1, buffer, len, MS2ST(100));
+ }
+
+ was_timeout = written != len;
+ if (was_timeout) {
+ write_timeout_cnt++;
+ }
+ }
}
void comm_usb_init(void) {
@@ -113,3 +133,7 @@ void comm_usb_send_packet(unsigned char *data, unsigned int len) {
packet_send_packet(data, len, PACKET_HANDLER);
chMtxUnlock(&send_mutex);
}
+
+unsigned int comm_usb_get_write_timeout_cnt(void) {
+ return write_timeout_cnt;
+}
diff --git a/comm_usb.h b/comm_usb.h
index 5ecf69544..2e539d193 100644
--- a/comm_usb.h
+++ b/comm_usb.h
@@ -25,5 +25,6 @@
// Functions
void comm_usb_init(void);
void comm_usb_send_packet(unsigned char *data, unsigned int len);
+unsigned int comm_usb_get_write_timeout_cnt(void);
#endif /* COMM_USB_H_ */
diff --git a/comm_usb_serial.c b/comm_usb_serial.c
index 20dfcd84c..aea880c68 100644
--- a/comm_usb_serial.c
+++ b/comm_usb_serial.c
@@ -250,11 +250,12 @@ static const USBEndpointConfig ep2config = {
NULL
};
+static volatile int configured_cnt = 0;
+
/*
* Handles the USB driver global events.
*/
static void usb_event(USBDriver *usbp, usbevent_t event) {
-
switch (event) {
case USB_EVENT_RESET:
return;
@@ -273,6 +274,7 @@ static void usb_event(USBDriver *usbp, usbevent_t event) {
sduConfigureHookI(&SDU1);
chSysUnlockFromISR();
+ configured_cnt++;
return;
case USB_EVENT_SUSPEND:
return;
@@ -331,3 +333,9 @@ void comm_usb_serial_init(void) {
int comm_usb_serial_is_active(void) {
return SDU1.config->usbp->state == USB_ACTIVE;
}
+
+// Every time the USB cable is plugged in a configuration is done. Unfortunately
+// I haven't found a way to detect when the USB cable gets unplugged.
+int comm_usb_serial_configured_cnt(void) {
+ return configured_cnt;
+}
diff --git a/comm_usb_serial.h b/comm_usb_serial.h
index bab2c3888..308326d2e 100644
--- a/comm_usb_serial.h
+++ b/comm_usb_serial.h
@@ -7,5 +7,6 @@ extern SerialUSBDriver SDU1;
// Functions
void comm_usb_serial_init(void);
int comm_usb_serial_is_active(void);
+int comm_usb_serial_configured_cnt(void);
#endif /* COMM_USB_SERIAL_H_ */
diff --git a/conf_general.h b/conf_general.h
index f33d90761..8aaf3d603 100644
--- a/conf_general.h
+++ b/conf_general.h
@@ -22,7 +22,7 @@
// Firmware version
#define FW_VERSION_MAJOR 3
-#define FW_VERSION_MINOR 65
+#define FW_VERSION_MINOR 66
#include "datatypes.h"
@@ -123,6 +123,9 @@
//#define HW_SOURCE "hw_rd2.c"
//#define HW_HEADER "hw_rd2.h"
+
+//#define HW_SOURCE "hw_100_250.c"
+//#define HW_HEADER "hw_100_250.h"
#endif
#ifndef HW_SOURCE
diff --git a/confgenerator.c b/confgenerator.c
index c4d4ae0da..0a8a88e0a 100644
--- a/confgenerator.c
+++ b/confgenerator.c
@@ -97,6 +97,8 @@ int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *
buffer[ind++] = conf->foc_temp_comp;
buffer_append_float32_auto(buffer, conf->foc_temp_comp_base_temp, &ind);
buffer_append_float32_auto(buffer, conf->foc_current_filter_const, &ind);
+ buffer[ind++] = conf->foc_cc_decoupling;
+ buffer[ind++] = conf->foc_observer_type;
buffer_append_int16(buffer, conf->gpd_buffer_notify_left, &ind);
buffer_append_int16(buffer, conf->gpd_buffer_interpol, &ind);
buffer_append_float32_auto(buffer, conf->gpd_current_filter_const, &ind);
@@ -363,6 +365,8 @@ bool confgenerator_deserialize_mcconf(const uint8_t *buffer, mc_configuration *c
conf->foc_temp_comp = buffer[ind++];
conf->foc_temp_comp_base_temp = buffer_get_float32_auto(buffer, &ind);
conf->foc_current_filter_const = buffer_get_float32_auto(buffer, &ind);
+ conf->foc_cc_decoupling = buffer[ind++];
+ conf->foc_observer_type = buffer[ind++];
conf->gpd_buffer_notify_left = buffer_get_int16(buffer, &ind);
conf->gpd_buffer_interpol = buffer_get_int16(buffer, &ind);
conf->gpd_current_filter_const = buffer_get_float32_auto(buffer, &ind);
@@ -625,6 +629,8 @@ void confgenerator_set_defaults_mcconf(mc_configuration *conf) {
conf->foc_temp_comp = MCCONF_FOC_TEMP_COMP;
conf->foc_temp_comp_base_temp = MCCONF_FOC_TEMP_COMP_BASE_TEMP;
conf->foc_current_filter_const = MCCONF_FOC_CURRENT_FILTER_CONST;
+ conf->foc_cc_decoupling = MCCONF_FOC_CC_DECOUPLING;
+ conf->foc_observer_type = MCCONF_FOC_OBSERVER_TYPE;
conf->gpd_buffer_notify_left = MCCONF_GPD_BUFFER_NOTIFY_LEFT;
conf->gpd_buffer_interpol = MCCONF_GPD_BUFFER_INTERPOL;
conf->gpd_current_filter_const = MCCONF_GPD_CURRENT_FILTER_CONST;
diff --git a/confgenerator.h b/confgenerator.h
index d9b7778a8..01940a4c4 100644
--- a/confgenerator.h
+++ b/confgenerator.h
@@ -8,7 +8,7 @@
#include <stdbool.h>
// Constants
-#define MCCONF_SIGNATURE 2967846088
+#define MCCONF_SIGNATURE 1671830952
#define APPCONF_SIGNATURE 783041200
// Functions
diff --git a/datatypes.h b/datatypes.h
index 9eb42b47a..c3eceae28 100644
--- a/datatypes.h
+++ b/datatypes.h
@@ -85,6 +85,19 @@ typedef enum {
MOTOR_TYPE_GPD
} mc_motor_type;
+// FOC current controller decoupling mode.
+typedef enum {
+ FOC_CC_DECOUPLING_DISABLED = 0,
+ FOC_CC_DECOUPLING_CROSS,
+ FOC_CC_DECOUPLING_BEMF,
+ FOC_CC_DECOUPLING_CROSS_BEMF
+} mc_foc_cc_decoupling_mode;
+
+typedef enum {
+ FOC_OBSERVER_ORTEGA_ORIGINAL = 0,
+ FOC_OBSERVER_ORTEGA_ITERATIVE
+} mc_foc_observer_type;
+
typedef enum {
FAULT_CODE_NONE = 0,
FAULT_CODE_OVER_VOLTAGE,
@@ -268,6 +281,8 @@ typedef struct {
bool foc_temp_comp;
float foc_temp_comp_base_temp;
float foc_current_filter_const;
+ mc_foc_cc_decoupling_mode foc_cc_decoupling;
+ mc_foc_observer_type foc_observer_type;
// GPDrive
int gpd_buffer_notify_left;
int gpd_buffer_interpol;
diff --git a/encoder.c b/encoder.c
index dd06a1f27..f18858d4a 100644
--- a/encoder.c
+++ b/encoder.c
@@ -80,7 +80,7 @@ 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;
-static uint16_t spi_val = 0;
+static uint32_t spi_val = 0;
static uint32_t spi_error_cnt = 0;
static float spi_error_rate = 0.0;
@@ -117,7 +117,7 @@ uint32_t encoder_spi_get_error_cnt(void) {
return spi_error_cnt;
}
-uint16_t encoder_spi_get_val(void) {
+uint32_t encoder_spi_get_val(void) {
return spi_val;
}
@@ -523,7 +523,7 @@ bool encoder_index_found(void) {
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;
+ uint16_t receive = 0;
for (int bit = 0;bit < 16;bit++) {
//palWritePad(HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI, send >> 15);
@@ -543,9 +543,9 @@ static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length)
__NOP();
samples += palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN);
- recieve <<= 1;
+ receive <<= 1;
if (samples > 2) {
- recieve |= 1;
+ receive |= 1;
}
palClearPad(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN);
@@ -553,7 +553,7 @@ static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length)
}
if (in_buf) {
- in_buf[i] = recieve;
+ in_buf[i] = receive;
}
}
}
@@ -672,6 +672,7 @@ static THD_FUNCTION(ts5700n8501_thread, arg) {
if (reply_ind == 6 && crc == reply[reply_ind - 1]) {
uint32_t pos = (uint32_t)reply[2] + ((uint32_t)reply[3] << 8) + ((uint32_t)reply[4] << 16);
+ spi_val = pos;
last_enc_angle = (float)pos / 131072.0 * 360.0;
UTILS_LP_FAST(spi_error_rate, 0.0, 1.0 / AS5047_SAMPLE_RATE_HZ);
} else {
diff --git a/encoder.h b/encoder.h
index dd40783aa..4dd471871 100644
--- a/encoder.h
+++ b/encoder.h
@@ -37,7 +37,7 @@ void encoder_tim_isr(void);
void encoder_set_counts(uint32_t counts);
bool encoder_index_found(void);
-uint16_t encoder_spi_get_val(void);
+uint32_t encoder_spi_get_val(void);
uint32_t encoder_spi_get_error_cnt(void);
float encoder_spi_get_error_rate(void);
uint32_t encoder_sincos_get_signal_below_min_error_cnt(void);
diff --git a/halconf.h b/halconf.h
index acb08b2b4..abbbf1ec6 100644
--- a/halconf.h
+++ b/halconf.h
@@ -306,7 +306,7 @@
* buffers.
*/
#if !defined(SERIAL_USB_BUFFERS_SIZE) || defined(__DOXYGEN__)
-#define SERIAL_USB_BUFFERS_SIZE 256
+#define SERIAL_USB_BUFFERS_SIZE 1024
#endif
/*===========================================================================*/
diff --git a/hwconf/drv8323s.c b/hwconf/drv8323s.c
index 78cef275f..211d8354f 100644
--- a/hwconf/drv8323s.c
+++ b/hwconf/drv8323s.c
@@ -355,15 +355,19 @@ static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length)
palClearPad(DRV8323S_SCK_GPIO, DRV8323S_SCK_PIN);
- int r1, r2, r3;
- r1 = palReadPad(DRV8323S_MISO_GPIO, DRV8323S_MISO_PIN);
+ int samples = 0;
+ samples += palReadPad(DRV8323S_MISO_GPIO, DRV8323S_MISO_PIN);
__NOP();
- r2 = palReadPad(DRV8323S_MISO_GPIO, DRV8323S_MISO_PIN);
+ samples += palReadPad(DRV8323S_MISO_GPIO, DRV8323S_MISO_PIN);
__NOP();
- r3 = palReadPad(DRV8323S_MISO_GPIO, DRV8323S_MISO_PIN);
+ samples += palReadPad(DRV8323S_MISO_GPIO, DRV8323S_MISO_PIN);
+ __NOP();
+ samples += palReadPad(DRV8323S_MISO_GPIO, DRV8323S_MISO_PIN);
+ __NOP();
+ samples += palReadPad(DRV8323S_MISO_GPIO, DRV8323S_MISO_PIN);
receive <<= 1;
- if (utils_middle_of_3_int(r1, r2, r3)) {
+ if (samples > 2) {
receive |= 1;
}
@@ -377,11 +381,15 @@ static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length)
}
static void spi_begin(void) {
+ spi_delay();
palClearPad(DRV8323S_CS_GPIO, DRV8323S_CS_PIN);
+ spi_delay();
}
static void spi_end(void) {
+ spi_delay();
palSetPad(DRV8323S_CS_GPIO, DRV8323S_CS_PIN);
+ spi_delay();
}
static void spi_delay(void) {
diff --git a/hwconf/hw.h b/hwconf/hw.h
index 8722870b5..0c039a556 100644
--- a/hwconf/hw.h
+++ b/hwconf/hw.h
@@ -199,6 +199,18 @@
#define IS_DRV_FAULT() 0
#endif
+// 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
+#ifndef CURR3_DOUBLE_SAMPLE
+#define CURR3_DOUBLE_SAMPLE 0
+#endif
+
#ifndef AUX_ON
#define AUX_ON()
#endif
diff --git a/hwconf/hw_100_250.c b/hwconf/hw_100_250.c
new file mode 100644
index 000000000..836faad29
--- /dev/null
+++ b/hwconf/hw_100_250.c
@@ -0,0 +1,255 @@
+/*
+ Copyright 2018 Benjamin Vedder benjamin@vedder.se
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "hw.h"
+
+#include "ch.h"
+#include "hal.h"
+#include "stm32f4xx_conf.h"
+#include "utils.h"
+#include <math.h>
+#include "mc_interface.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);
+ RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
+
+ // LEDs
+ palSetPadMode(LED_GREEN_GPIO, LED_GREEN_PIN,
+ PAL_MODE_OUTPUT_PUSHPULL |
+ PAL_STM32_OSPEED_HIGHEST);
+ palSetPadMode(LED_RED_GPIO, LED_RED_PIN,
+ 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);
+
+ // Phase filters
+ palSetPadMode(PHASE_FILTER_GPIO, PHASE_FILTER_PIN,
+ PAL_MODE_OUTPUT_PUSHPULL |
+ PAL_STM32_OSPEED_HIGHEST);
+ PHASE_FILTER_OFF();
+
+ // AUX pin
+ AUX_OFF();
+ palSetPadMode(AUX_GPIO, AUX_PIN,
+ PAL_MODE_OUTPUT_PUSHPULL |
+ PAL_STM32_OSPEED_HIGHEST);
+
+ // 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, 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, 4, PAL_MODE_INPUT_ANALOG);
+ palSetPadMode(GPIOC, 5, PAL_MODE_INPUT_ANALOG);
+}
+
+void hw_setup_adc_channels(void) {
+ // ADC1 regular channels
+ ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_15Cycles);
+ ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 2, ADC_SampleTime_15Cycles);
+ ADC_RegularChannelConfig(ADC1, ADC_Channel_5, 3, ADC_SampleTime_15Cycles);
+ ADC_RegularChannelConfig(ADC1, ADC_Channel_14, 4, ADC_SampleTime_15Cycles);
+ ADC_RegularChannelConfig(ADC1, ADC_Channel_Vrefint, 5, ADC_SampleTime_15Cycles);
+ ADC_RegularChannelConfig(ADC1, ADC_Channel_8, 6, ADC_SampleTime_15Cycles);
+
+ // ADC2 regular channels
+ ADC_RegularChannelConfig(ADC2, ADC_Channel_1, 1, ADC_SampleTime_15Cycles);
+ ADC_RegularChannelConfig(ADC2, ADC_Channel_11, 2, ADC_SampleTime_15Cycles);
+ ADC_RegularChannelConfig(ADC2, ADC_Channel_6, 3, ADC_SampleTime_15Cycles);
+ ADC_RegularChannelConfig(ADC2, ADC_Channel_15, 4, ADC_SampleTime_15Cycles);
+ ADC_RegularChannelConfig(ADC2, ADC_Channel_0, 5, ADC_SampleTime_15Cycles);
+ ADC_RegularChannelConfig(ADC2, ADC_Channel_9, 6, ADC_SampleTime_15Cycles);
+
+ // ADC3 regular channels
+ ADC_RegularChannelConfig(ADC3, ADC_Channel_2, 1, ADC_SampleTime_15Cycles);
+ ADC_RegularChannelConfig(ADC3, ADC_Channel_12, 2, ADC_SampleTime_15Cycles);
+ ADC_RegularChannelConfig(ADC3, ADC_Channel_3, 3, ADC_SampleTime_15Cycles);
+ ADC_RegularChannelConfig(ADC3, ADC_Channel_13, 4, ADC_SampleTime_15Cycles);
+ ADC_RegularChannelConfig(ADC3, ADC_Channel_1, 5, ADC_SampleTime_15Cycles);
+ ADC_RegularChannelConfig(ADC3, ADC_Channel_2, 6, ADC_SampleTime_15Cycles);
+
+ // Injected channels
+ ADC_InjectedChannelConfig(ADC1, ADC_Channel_10, 1, ADC_SampleTime_15Cycles);
+ ADC_InjectedChannelConfig(ADC2, ADC_Channel_11, 1, ADC_SampleTime_15Cycles);
+ ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 1, ADC_SampleTime_15Cycles);
+ ADC_InjectedChannelConfig(ADC1, ADC_Channel_10, 2, ADC_SampleTime_15Cycles);
+ ADC_InjectedChannelConfig(ADC2, ADC_Channel_11, 2, ADC_SampleTime_15Cycles);
+ ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 2, ADC_SampleTime_15Cycles);
+ ADC_InjectedChannelConfig(ADC1, ADC_Channel_10, 3, ADC_SampleTime_15Cycles);
+ ADC_InjectedChannelConfig(ADC2, ADC_Channel_11, 3, ADC_SampleTime_15Cycles);
+ ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 3, ADC_SampleTime_15Cycles);
+}
+
+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);
+ }
+}
+
+float hw100_250_get_temp(void) {
+ float t1 = (1.0 / ((logf(NTC_RES(ADC_Value[ADC_IND_TEMP_MOS]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15);
+ float t2 = (1.0 / ((logf(NTC_RES(ADC_Value[ADC_IND_TEMP_MOS_2]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15);
+ float t3 = (1.0 / ((logf(NTC_RES(ADC_Value[ADC_IND_TEMP_MOS_3]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15);
+ float res = 0.0;
+
+ if (t1 > t2 && t1 > t3) {
+ res = t1;
+ } else if (t2 > t1 && t2 > t3) {
+ res = t2;
+ } else {
+ res = t3;
+ }
+
+ return res;
+}
diff --git a/hwconf/hw_100_250.h b/hwconf/hw_100_250.h
new file mode 100644
index 000000000..731c49fdc
--- /dev/null
+++ b/hwconf/hw_100_250.h
@@ -0,0 +1,260 @@
+/*
+ Copyright 2018 Benjamin Vedder benjamin@vedder.se
+
+ This file is part of the VESC firmware.
+
+ The VESC firmware 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.
+
+ The VESC firmware is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef HW_100_250_H_
+#define HW_100_250_H_
+
+#define HW_NAME "100_250"
+
+// HW properties
+#define HW_HAS_3_SHUNTS
+#define INVERTED_SHUNT_POLARITY
+
+// Macros
+#define LED_GREEN_GPIO GPIOB
+#define LED_GREEN_PIN 5
+#define LED_RED_GPIO GPIOB
+#define LED_RED_PIN 7
+
+#define LED_GREEN_ON() palSetPad(LED_GREEN_GPIO, LED_GREEN_PIN)
+#define LED_GREEN_OFF() palClearPad(LED_GREEN_GPIO, LED_GREEN_PIN)
+#define LED_RED_ON() palSetPad(LED_RED_GPIO, LED_RED_PIN)
+#define LED_RED_OFF() palClearPad(LED_RED_GPIO, LED_RED_PIN)
+
+#define PHASE_FILTER_GPIO GPIOC
+#define PHASE_FILTER_PIN 9
+#define PHASE_FILTER_ON() palSetPad(PHASE_FILTER_GPIO, PHASE_FILTER_PIN)
+#define PHASE_FILTER_OFF() palClearPad(PHASE_FILTER_GPIO, PHASE_FILTER_PIN)
+
+#define AUX_GPIO GPIOC
+#define AUX_PIN 12
+#define AUX_ON() palSetPad(AUX_GPIO, AUX_PIN)
+#define AUX_OFF() palClearPad(AUX_GPIO, AUX_PIN)
+
+/*
+ * ADC Vector
+ *
+ * 0 (1): IN0 SENS1
+ * 1 (2): IN1 SENS2
+ * 2 (3): IN2 SENS3
+ * 3 (1): IN10 CURR1
+ * 4 (2): IN11 CURR2
+ * 5 (3): IN12 CURR3
+ * 6 (1): IN5 ADC_EXT1
+ * 7 (2): IN6 ADC_EXT2
+ * 8 (3): IN3 TEMP_MOS
+ * 9 (1): IN14 TEMP_MOTOR
+ * 10 (2): IN15 ADC_EXT3
+ * 11 (3): IN13 AN_IN
+ * 12 (1): Vrefint
+ * 13 (2): IN0 SENS1
+ * 14 (3): IN1 SENS2
+ * 15 (1): IN8 TEMP_MOS_2
+ * 16 (2): IN9 TEMP_MOS_3
+ * 17 (3): IN3 SENS3
+ */
+
+#define HW_ADC_CHANNELS 18
+#define HW_ADC_INJ_CHANNELS 3
+#define HW_ADC_NBR_CONV 6
+
+// ADC Indexes
+#define ADC_IND_SENS1 0
+#define ADC_IND_SENS2 1
+#define ADC_IND_SENS3 2
+#define ADC_IND_CURR1 3
+#define ADC_IND_CURR2 4
+#define ADC_IND_CURR3 5
+#define ADC_IND_VIN_SENS 11
+#define ADC_IND_EXT 6
+#define ADC_IND_EXT2 7
+#define ADC_IND_EXT3 10
+#define ADC_IND_TEMP_MOS 8
+#define ADC_IND_TEMP_MOS_2 15
+#define ADC_IND_TEMP_MOS_3 16
+#define ADC_IND_TEMP_MOTOR 9
+#define ADC_IND_VREFINT 12
+
+// ADC macros and settings
+
+// Component parameters (can be overridden)
+#ifndef V_REG
+#define V_REG 3.44
+#endif
+#ifndef VIN_R1
+#define VIN_R1 150000.0
+#endif
+#ifndef VIN_R2
+#define VIN_R2 4700.0
+#endif
+#ifndef CURRENT_AMP_GAIN
+#define CURRENT_AMP_GAIN 20.0
+#endif
+#ifndef CURRENT_SHUNT_RES
+#define CURRENT_SHUNT_RES (0.0005 / 3.0)
+#endif
+
+// Input voltage
+#define GET_INPUT_VOLTAGE() ((V_REG / 4095.0) * (float)ADC_Value[ADC_IND_VIN_SENS] * ((VIN_R1 + VIN_R2) / VIN_R2))
+
+// NTC Termistors
+#define NTC_RES(adc_val) ((4095.0 * 10000.0) / adc_val - 10000.0)
+#define NTC_TEMP(adc_ind) hw100_250_get_temp()
+
+#define NTC_RES_MOTOR(adc_val) (10000.0 / ((4095.0 / (float)adc_val) - 1.0)) // Motor temp sensor on low side
+#define NTC_TEMP_MOTOR(beta) (1.0 / ((logf(NTC_RES_MOTOR(ADC_Value[ADC_IND_TEMP_MOTOR]) / 10000.0) / beta) + (1.0 / 298.15)) - 273.15)
+
+#define NTC_TEMP_MOS1() (1.0 / ((logf(NTC_RES(ADC_Value[ADC_IND_TEMP_MOS]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15)
+#define NTC_TEMP_MOS2() (1.0 / ((logf(NTC_RES(ADC_Value[ADC_IND_TEMP_MOS_2]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15)
+#define NTC_TEMP_MOS3() (1.0 / ((logf(NTC_RES(ADC_Value[ADC_IND_TEMP_MOS_3]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15)
+
+// Voltage on ADC channel
+#define ADC_VOLTS(ch) ((float)ADC_Value[ch] / 4096.0 * V_REG)
+
+// COMM-port ADC GPIOs
+#define HW_ADC_EXT_GPIO GPIOA
+#define HW_ADC_EXT_PIN 5
+#define HW_ADC_EXT2_GPIO GPIOA
+#define HW_ADC_EXT2_PIN 6
+
+// UART Peripheral
+#define HW_UART_DEV SD3
+#define HW_UART_GPIO_AF GPIO_AF_USART3
+#define HW_UART_TX_PORT GPIOB
+#define HW_UART_TX_PIN 10
+#define HW_UART_RX_PORT GPIOB
+#define HW_UART_RX_PIN 11
+
+// Permanent UART Peripheral (for NRF52)
+#define HW_UART_P_BAUD 115200
+#define HW_UART_P_DEV SD4
+#define HW_UART_P_GPIO_AF GPIO_AF_UART4
+#define HW_UART_P_TX_PORT GPIOC
+#define HW_UART_P_TX_PIN 10
+#define HW_UART_P_RX_PORT GPIOC
+#define HW_UART_P_RX_PIN 11
+
+// ICU Peripheral for servo decoding
+#define HW_USE_SERVO_TIM4
+#define HW_ICU_TIMER TIM4
+#define HW_ICU_TIM_CLK_EN() RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE)
+#define HW_ICU_DEV ICUD4
+#define HW_ICU_CHANNEL ICU_CHANNEL_1
+#define HW_ICU_GPIO_AF GPIO_AF_TIM4
+#define HW_ICU_GPIO GPIOB
+#define HW_ICU_PIN 6
+
+// 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 GPIOC
+#define HW_HALL_ENC_PIN1 6
+#define HW_HALL_ENC_GPIO2 GPIOC
+#define HW_HALL_ENC_PIN2 7
+#define HW_HALL_ENC_GPIO3 GPIOC
+#define HW_HALL_ENC_PIN3 8
+#define HW_ENC_TIM TIM3
+#define HW_ENC_TIM_AF GPIO_AF_TIM3
+#define HW_ENC_TIM_CLK_EN() RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE)
+#define HW_ENC_EXTI_PORTSRC EXTI_PortSourceGPIOC
+#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
+#define HW_ENC_TIM_ISR_CH TIM3_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM3_IRQHandler
+
+// SPI pins
+#define HW_SPI_DEV SPID1
+#define HW_SPI_GPIO_AF GPIO_AF_SPI1
+#define HW_SPI_PORT_NSS GPIOA
+#define HW_SPI_PIN_NSS 4
+#define HW_SPI_PORT_SCK GPIOA
+#define HW_SPI_PIN_SCK 5
+#define HW_SPI_PORT_MOSI GPIOA
+#define HW_SPI_PIN_MOSI 7
+#define HW_SPI_PORT_MISO GPIOA
+#define HW_SPI_PIN_MISO 6
+
+// NRF SWD
+#define NRF5x_SWDIO_GPIO GPIOA
+#define NRF5x_SWDIO_PIN 15
+#define NRF5x_SWCLK_GPIO GPIOB
+#define NRF5x_SWCLK_PIN 3
+
+// 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)
+
+// Override dead time. See the stm32f4 reference manual for calculating this value.
+#define HW_DEAD_TIME_NSEC 660.0
+
+// Default setting overrides
+#ifndef MCCONF_L_MIN_VOLTAGE
+#define MCCONF_L_MIN_VOLTAGE 12.0 // Minimum input voltage
+#endif
+#ifndef MCCONF_L_MAX_VOLTAGE
+#define MCCONF_L_MAX_VOLTAGE 96.0 // Maximum input voltage
+#endif
+#ifndef MCCONF_DEFAULT_MOTOR_TYPE
+#define MCCONF_DEFAULT_MOTOR_TYPE MOTOR_TYPE_FOC
+#endif
+#ifndef MCCONF_FOC_F_SW
+#define MCCONF_FOC_F_SW 30000.0
+#endif
+#ifndef MCCONF_L_MAX_ABS_CURRENT
+#define MCCONF_L_MAX_ABS_CURRENT 400.0 // The maximum absolute current above which a fault is generated
+#endif
+#ifndef MCCONF_FOC_SAMPLE_V0_V7
+#define MCCONF_FOC_SAMPLE_V0_V7 false // Run control loop in both v0 and v7 (requires phase shunts)
+#endif
+#ifndef MCCONF_L_IN_CURRENT_MAX
+#define MCCONF_L_IN_CURRENT_MAX 250.0 // Input current limit in Amperes (Upper)
+#endif
+#ifndef MCCONF_L_IN_CURRENT_MIN
+#define MCCONF_L_IN_CURRENT_MIN -200.0 // Input current limit in Amperes (Lower)
+#endif
+
+// Setting limits
+#define HW_LIM_CURRENT -300.0, 300.0
+#define HW_LIM_CURRENT_IN -300.0, 300.0
+#define HW_LIM_CURRENT_ABS 0.0, 420.0
+#define HW_LIM_VIN 11.0, 97.0
+#define HW_LIM_ERPM -200e3, 200e3
+#define HW_LIM_DUTY_MIN 0.0, 0.1
+#define HW_LIM_DUTY_MAX 0.0, 0.99
+#define HW_LIM_TEMP_FET -40.0, 110.0
+
+// HW-specific functions
+float hw100_250_get_temp(void);
+
+#endif /* HW_100_250_H_ */
diff --git a/hwconf/hw_75_300.h b/hwconf/hw_75_300.h
index c1889c25e..5534259d7 100644
--- a/hwconf/hw_75_300.h
+++ b/hwconf/hw_75_300.h
@@ -260,6 +260,9 @@
#define HW_DEAD_TIME_NSEC 660.0
// Default setting overrides
+#ifndef MCCONF_L_MIN_VOLTAGE
+#define MCCONF_L_MIN_VOLTAGE 12.0 // Minimum input voltage
+#endif
#ifndef MCCONF_L_MAX_VOLTAGE
#define MCCONF_L_MAX_VOLTAGE 72.0 // Maximum input voltage
#endif
@@ -283,10 +286,10 @@
#endif
// Setting limits
-#define HW_LIM_CURRENT -300.0, 300.0
-#define HW_LIM_CURRENT_IN -300.0, 300.0
-#define HW_LIM_CURRENT_ABS 0.0, 450.0
-#define HW_LIM_VIN 6.0, 72.0
+#define HW_LIM_CURRENT -400.0, 400.0
+#define HW_LIM_CURRENT_IN -400.0, 400.0
+#define HW_LIM_CURRENT_ABS 0.0, 480.0
+#define HW_LIM_VIN 11.0, 72.0
#define HW_LIM_ERPM -200e3, 200e3
#define HW_LIM_DUTY_MIN 0.0, 0.1
#define HW_LIM_DUTY_MAX 0.0, 0.99
diff --git a/mcconf/mcconf_default.h b/mcconf/mcconf_default.h
index ac0771657..bc3f6ae1e 100644
--- a/mcconf/mcconf_default.h
+++ b/mcconf/mcconf_default.h
@@ -225,10 +225,10 @@
#define MCCONF_FOC_CURRENT_KI 50.0
#endif
#ifndef MCCONF_FOC_F_SW
-#define MCCONF_FOC_F_SW 20000.0
+#define MCCONF_FOC_F_SW 25000.0
#endif
#ifndef MCCONF_FOC_DT_US
-#define MCCONF_FOC_DT_US 0.08 // Microseconds for dead time compensation
+#define MCCONF_FOC_DT_US 0.12 // Microseconds for dead time compensation
#endif
#ifndef MCCONF_FOC_ENCODER_INVERTED
#define MCCONF_FOC_ENCODER_INVERTED false
@@ -329,10 +329,16 @@
#ifndef MCCONF_FOC_CURRENT_FILTER_CONST
#define MCCONF_FOC_CURRENT_FILTER_CONST 0.1 // Filter constant for the filtered currents
#endif
+#ifndef MCCONF_FOC_CC_DECOUPLING
+#define MCCONF_FOC_CC_DECOUPLING FOC_CC_DECOUPLING_BEMF // Current controller decoupling
+#endif
+#ifndef MCCONF_FOC_OBSERVER_TYPE
+#define MCCONF_FOC_OBSERVER_TYPE FOC_OBSERVER_ORTEGA_ORIGINAL // Position observer type for FOC
+#endif
// GPD
#ifndef MCCONF_GPD_BUFFER_NOTIFY_LEFT
-#define MCCONF_GPD_BUFFER_NOTIFY_LEFT 200 // Notify when the buffer space is left than this
+#define MCCONF_GPD_BUFFER_NOTIFY_LEFT 200 // Notify when the buffer space left is less than this
#endif
#ifndef MCCONF_GPD_BUFFER_INTERPOL
#define MCCONF_GPD_BUFFER_INTERPOL 0 // Buffer interpolation
diff --git a/mcpwm.c b/mcpwm.c
index 8780082af..a9fc7af23 100644
--- a/mcpwm.c
+++ b/mcpwm.c
@@ -1467,6 +1467,17 @@ void mcpwm_adc_inj_int_handler(void) {
int curr2 = ADC_GetInjectedConversionValue(ADC3, ADC_InjectedChannel_1);
#endif
+#ifdef INVERTED_SHUNT_POLARITY
+ curr0 = 4095 - curr0;
+ curr1 = 4095 - curr1;
+
+ curr0_2 = 4095 - curr0_2;
+ curr1_2 = 4095 - curr1_2;
+#ifdef HW_HAS_3_SHUNTS
+ curr2 = 4095 - curr2;
+#endif
+#endif
+
float curr0_currsamp = curr0;
float curr1_currsamp = curr1;
#ifdef HW_HAS_3_SHUNTS
diff --git a/mcpwm_foc.c b/mcpwm_foc.c
index 2b0b413d6..348d9e1c2 100644
--- a/mcpwm_foc.c
+++ b/mcpwm_foc.c
@@ -64,6 +64,7 @@ typedef struct {
float vq;
float vd_int;
float vq_int;
+ float speed_rad_s;
uint32_t svm_sector;
} motor_state_t;
@@ -825,7 +826,7 @@ bool mcpwm_foc_is_using_encoder(void) {
* The RPM value.
*/
float mcpwm_foc_get_rpm(void) {
- return m_pll_speed / ((2.0 * M_PI) / 60.0);
+ return m_motor_state.speed_rad_s / ((2.0 * M_PI) / 60.0);
}
/**
@@ -1987,6 +1988,8 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
observer_update(m_motor_state.v_alpha, m_motor_state.v_beta,
m_motor_state.i_alpha, m_motor_state.i_beta, dt,
&m_observer_x1, &m_observer_x2, &m_phase_now_observer);
+ m_phase_now_observer += m_pll_speed * dt * 0.5;
+ utils_norm_angle_rad((float*)&m_phase_now_observer);
}
switch (m_conf->foc_sensor_mode) {
@@ -2073,6 +2076,17 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
control_current(&m_motor_state, dt);
} else {
+ // The current is 0 when the motor is undriven
+ m_motor_state.i_alpha = 0.0;
+ m_motor_state.i_beta = 0.0;
+ m_motor_state.id = 0.0;
+ m_motor_state.iq = 0.0;
+ m_motor_state.id_filter = 0.0;
+ m_motor_state.iq_filter = 0.0;
+ m_motor_state.i_bus = 0.0;
+ m_motor_state.i_abs = 0.0;
+ m_motor_state.i_abs_filter = 0.0;
+
// Track back emf
#ifdef HW_HAS_3_SHUNTS
float Va = ADC_VOLTS(ADC_IND_SENS1) * ((VIN_R1 + VIN_R2) / VIN_R2);
@@ -2088,9 +2102,6 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
m_motor_state.v_alpha = (2.0 / 3.0) * Va - (1.0 / 3.0) * Vb - (1.0 / 3.0) * Vc;
m_motor_state.v_beta = ONE_BY_SQRT3 * Vb - ONE_BY_SQRT3 * Vc;
- float c, s;
- utils_fast_sincos_better(m_motor_state.phase, &s, &c);
-
#ifdef HW_USE_LINE_TO_LINE
// rotate alpha-beta 30 degrees to compensate for line-to-line phase voltage sensing
float x_tmp = m_motor_state.v_alpha;
@@ -2104,38 +2115,16 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
m_motor_state.v_beta *= ONE_BY_SQRT3;
#endif
- // Park transform
- float vd_tmp = c * m_motor_state.v_alpha + s * m_motor_state.v_beta;
- float vq_tmp = c * m_motor_state.v_beta - s * m_motor_state.v_alpha;
-
- UTILS_NAN_ZERO(m_motor_state.vd);
- UTILS_NAN_ZERO(m_motor_state.vq);
-
- UTILS_LP_FAST(m_motor_state.vd, vd_tmp, 0.2);
- UTILS_LP_FAST(m_motor_state.vq, vq_tmp, 0.2);
-
- m_motor_state.vd_int = m_motor_state.vd;
- m_motor_state.vq_int = m_motor_state.vq;
-
- // Update corresponding modulation
- m_motor_state.mod_d = m_motor_state.vd / ((2.0 / 3.0) * m_motor_state.v_bus);
- m_motor_state.mod_q = m_motor_state.vq / ((2.0 / 3.0) * m_motor_state.v_bus);
-
- // The current is 0 when the motor is undriven
- m_motor_state.i_alpha = 0.0;
- m_motor_state.i_beta = 0.0;
- m_motor_state.id = 0.0;
- m_motor_state.iq = 0.0;
- m_motor_state.id_filter = 0.0;
- m_motor_state.iq_filter = 0.0;
- m_motor_state.i_bus = 0.0;
- m_motor_state.i_abs = 0.0;
- m_motor_state.i_abs_filter = 0.0;
-
// Run observer
+ static float x1_prev = 0.0;
+ static float x2_prev = 0.0;
observer_update(m_motor_state.v_alpha, m_motor_state.v_beta,
m_motor_state.i_alpha, m_motor_state.i_beta, dt, &m_observer_x1,
- &m_observer_x2, &m_phase_now_observer);
+ &m_observer_x2, 0);
+ m_phase_now_observer = utils_fast_atan2(x2_prev + m_observer_x2, x1_prev + m_observer_x1);
+
+ x1_prev = m_observer_x1;
+ x2_prev = m_observer_x2;
switch (m_conf->foc_sensor_mode) {
case FOC_SENSOR_MODE_ENCODER:
@@ -2149,15 +2138,43 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
m_motor_state.phase = m_phase_now_observer;
break;
}
+
+ float c, s;
+ utils_fast_sincos_better(m_motor_state.phase, &s, &c);
+
+ // Park transform
+ float vd_tmp = c * m_motor_state.v_alpha + s * m_motor_state.v_beta;
+ float vq_tmp = c * m_motor_state.v_beta - s * m_motor_state.v_alpha;
+
+ UTILS_NAN_ZERO(m_motor_state.vd);
+ UTILS_NAN_ZERO(m_motor_state.vq);
+
+ UTILS_LP_FAST(m_motor_state.vd, vd_tmp, 0.2);
+ UTILS_LP_FAST(m_motor_state.vq, vq_tmp, 0.2);
+
+ // Set the current controller integrator to the BEMF voltage to avoid
+ // a current spike when the motor is driven again. Notice that we have
+ // to take decoupling into account.
+ m_motor_state.vd_int = m_motor_state.vd;
+ m_motor_state.vq_int = m_motor_state.vq;
+
+ if (m_conf->foc_cc_decoupling == FOC_CC_DECOUPLING_BEMF || m_conf->foc_cc_decoupling == FOC_CC_DECOUPLING_CROSS_BEMF) {
+ m_motor_state.vq_int -= m_motor_state.speed_rad_s * m_conf->foc_motor_flux_linkage;
+ }
+
+ // Update corresponding modulation
+ m_motor_state.mod_d = m_motor_state.vd / ((2.0 / 3.0) * m_motor_state.v_bus);
+ m_motor_state.mod_q = m_motor_state.vq / ((2.0 / 3.0) * m_motor_state.v_bus);
}
// Calculate duty cycle
m_motor_state.duty_now = SIGN(m_motor_state.vq) *
- sqrtf(m_motor_state.mod_d * m_motor_state.mod_d +
- m_motor_state.mod_q * m_motor_state.mod_q) / SQRT3_BY_2;
+ sqrtf(SQ(m_motor_state.mod_d) + SQ(m_motor_state.mod_q))
+ / SQRT3_BY_2;
// Run PLL for speed estimation
pll_run(m_motor_state.phase, dt, &m_pll_phase, &m_pll_speed);
+ m_motor_state.speed_rad_s = m_pll_speed;
// Update tachometer (resolution = 60 deg as for BLDC)
float ph_tmp = m_motor_state.phase;
@@ -2344,7 +2361,6 @@ void observer_update(float v_alpha, float v_beta, float i_alpha, float i_beta,
float dt, volatile float *x1, volatile float *x2, volatile float *phase) {
const float L = (3.0 / 2.0) * m_conf->foc_motor_l;
- const float lambda = m_conf->foc_motor_flux_linkage;
float R = (3.0 / 2.0) * m_conf->foc_motor_r;
// Saturation compensation
@@ -2353,7 +2369,7 @@ void observer_update(float v_alpha, float v_beta, float i_alpha, float i_beta,
// Temperature compensation
const float t = mc_interface_temp_motor_filtered();
- if (m_conf->foc_temp_comp && t > -5.0) {
+ if (m_conf->foc_temp_comp && t > -25.0) {
R += R * 0.00386 * (t - m_conf->foc_temp_comp_base_temp);
}
@@ -2361,33 +2377,41 @@ void observer_update(float v_alpha, float v_beta, float i_alpha, float i_beta,
const float L_ib = L * i_beta;
const float R_ia = R * i_alpha;
const float R_ib = R * i_beta;
- const float lambda_2 = SQ(lambda);
+ const float lambda_2 = SQ(m_conf->foc_motor_flux_linkage);
const float gamma_half = m_gamma_now * 0.5;
- // Original
-// float err = lambda_2 - (SQ(*x1 - L_ia) + SQ(*x2 - L_ib));
-// float x1_dot = -R_ia + v_alpha + gamma_half * (*x1 - L_ia) * err;
-// float x2_dot = -R_ib + v_beta + gamma_half * (*x2 - L_ib) * err;
-// *x1 += x1_dot * dt;
-// *x2 += x2_dot * dt;
+ switch (m_conf->foc_observer_type) {
+ case FOC_OBSERVER_ORTEGA_ORIGINAL: {
+ float err = lambda_2 - (SQ(*x1 - L_ia) + SQ(*x2 - L_ib));
+ float x1_dot = -R_ia + v_alpha + gamma_half * (*x1 - L_ia) * err;
+ float x2_dot = -R_ib + v_beta + gamma_half * (*x2 - L_ib) * err;
+ *x1 += x1_dot * dt;
+ *x2 += x2_dot * dt;
+ } break;
+
+ case FOC_OBSERVER_ORTEGA_ITERATIVE: {
+ // Iterative with some trial and error
+ const int iterations = 6;
+ const float dt_iteration = dt / (float)iterations;
+ for (int i = 0;i < iterations;i++) {
+ float err = lambda_2 - (SQ(*x1 - L_ia) + SQ(*x2 - L_ib));
+ float gamma_tmp = gamma_half;
+ if (utils_truncate_number_abs(&err, lambda_2 * 0.2)) {
+ gamma_tmp *= 10.0;
+ }
+ float x1_dot = -R_ia + v_alpha + gamma_tmp * (*x1 - L_ia) * err;
+ float x2_dot = -R_ib + v_beta + gamma_tmp * (*x2 - L_ib) * err;
- // Iterative with some trial and error
- const int iterations = 6;
- const float dt_iteration = dt / (float)iterations;
- for (int i = 0;i < iterations;i++) {
- float err = lambda_2 - (SQ(*x1 - L_ia) + SQ(*x2 - L_ib));
- float gamma_tmp = gamma_half;
- if (utils_truncate_number_abs(&err, lambda_2 * 0.2)) {
- gamma_tmp *= 10.0;
- }
- float x1_dot = -R_ia + v_alpha + gamma_tmp * (*x1 - L_ia) * err;
- float x2_dot = -R_ib + v_beta + gamma_tmp * (*x2 - L_ib) * err;
+ *x1 += x1_dot * dt_iteration;
+ *x2 += x2_dot * dt_iteration;
+ }
+ } break;
- *x1 += x1_dot * dt_iteration;
- *x2 += x2_dot * dt_iteration;
+ default:
+ break;
}
- // Same as above, but without iterations.
+ // Same as iterative, but without iterations.
// float err = lambda_2 - (SQ(*x1 - L_ia) + SQ(*x2 - L_ib));
// float gamma_tmp = gamma_half;
// if (utils_truncate_number_abs(&err, lambda_2 * 0.2)) {
@@ -2401,7 +2425,9 @@ void observer_update(float v_alpha, float v_beta, float i_alpha, float i_beta,
UTILS_NAN_ZERO(*x1);
UTILS_NAN_ZERO(*x2);
- *phase = utils_fast_atan2(*x2 - L_ib, *x1 - L_ia);
+ if (phase) {
+ *phase = utils_fast_atan2(*x2 - L_ib, *x1 - L_ia);
+ }
}
static void pll_run(float phase, float dt, volatile float *phase_var,
@@ -2429,6 +2455,7 @@ static void pll_run(float phase, float dt, volatile float *phase_var,
* i_alpha
* i_beta
* v_bus
+ * speed_rad_s
*
* Parameters that will be updated in this function:
* i_bus
@@ -2479,18 +2506,60 @@ static void control_current(volatile motor_state_t *state_m, float dt) {
state_m->vd_int += Ierr_d * (ki * dt);
state_m->vq_int += Ierr_q * (ki * dt);
- // Saturation
- utils_saturate_vector_2d((float*)&state_m->vd, (float*)&state_m->vq,
- (2.0 / 3.0) * max_duty * SQRT3_BY_2 * state_m->v_bus);
+ // Decoupling
+ float dec_vd = 0.0;
+ float dec_vq = 0.0;
+
+ if (m_control_mode < CONTROL_MODE_HANDBRAKE && m_conf->foc_cc_decoupling != FOC_CC_DECOUPLING_DISABLED) {
+ switch (m_conf->foc_cc_decoupling) {
+ case FOC_CC_DECOUPLING_CROSS:
+ dec_vd = state_m->iq * state_m->speed_rad_s * m_conf->foc_motor_l;
+ dec_vq = state_m->id * state_m->speed_rad_s * m_conf->foc_motor_l;
+ break;
+
+ case FOC_CC_DECOUPLING_BEMF:
+ dec_vq = state_m->speed_rad_s * m_conf->foc_motor_flux_linkage;
+ break;
+
+ case FOC_CC_DECOUPLING_CROSS_BEMF:
+ dec_vd = state_m->iq * state_m->speed_rad_s * m_conf->foc_motor_l;
+ dec_vq = state_m->id * state_m->speed_rad_s * m_conf->foc_motor_l + state_m->speed_rad_s * m_conf->foc_motor_flux_linkage;
+ break;
+
+ default:
+ break;
+ }
+ }
+
+ state_m->vd -= dec_vd;
+ state_m->vq += dec_vq;
+ float max_v_mag = (2.0 / 3.0) * max_duty * SQRT3_BY_2 * state_m->v_bus;
+
+ // Saturation
+ utils_saturate_vector_2d((float*)&state_m->vd, (float*)&state_m->vq, max_v_mag);
state_m->mod_d = state_m->vd / ((2.0 / 3.0) * state_m->v_bus);
state_m->mod_q = state_m->vq / ((2.0 / 3.0) * state_m->v_bus);
- // Windup protection
-// utils_saturate_vector_2d((float*)&state_m->vd_int, (float*)&state_m->vq_int,
-// (2.0 / 3.0) * max_duty * SQRT3_BY_2 * state_m->v_bus);
- utils_truncate_number_abs((float*)&state_m->vd_int, (2.0 / 3.0) * max_duty * SQRT3_BY_2 * state_m->v_bus);
- utils_truncate_number_abs((float*)&state_m->vq_int, (2.0 / 3.0) * max_duty * SQRT3_BY_2 * state_m->v_bus);
+ // Integrator windup protection
+ // This is important, tricky and probably needs improvement.
+ // Currently we check if the integrator causes over modulation and decrease the components
+ // that contribute to the over modulation.
+ float int_over_mod = sqrtf(SQ(state_m->vd_int - dec_vd) + SQ(state_m->vq_int + dec_vq)) - max_v_mag;
+ float max_int_over_mod = -0.1;
+ if (int_over_mod > max_int_over_mod) {
+ float diff_vd = fabsf((state_m->vd_int - dec_vd)) - fabsf(dec_vd);
+ float diff_vq = fabsf((state_m->vq_int + dec_vq)) - fabsf(dec_vq);
+ int_over_mod -= max_int_over_mod;
+ float dec_gain = 0.01;
+
+ if (diff_vd > 0.0) {
+ state_m->vd_int *= 1.0 - dec_gain * int_over_mod;
+ }
+ if (diff_vq > 0.0) {
+ state_m->vq_int *= 1.0 - dec_gain * int_over_mod;
+ }
+ }
// TODO: Have a look at this?
state_m->i_bus = state_m->mod_d * state_m->id + state_m->mod_q * state_m->iq;
@@ -2522,7 +2591,8 @@ static void control_current(volatile motor_state_t *state_m, float dt) {
svm(-mod_alpha, -mod_beta, top, &duty1, &duty2, &duty3, (uint32_t*)&state_m->svm_sector);
TIMER_UPDATE_DUTY(duty1, duty2, duty3);
- if(virtual_motor_is_connected() == false){//do not allow to turn on PWM outputs if virtual motor is used
+ // do not allow to turn on PWM outputs if virtual motor is used
+ if(virtual_motor_is_connected() == false) {
if (!m_output_on) {
start_pwm_hw();
}
diff --git a/terminal.c b/terminal.c
index 537b7bda9..e778bcfea 100644
--- a/terminal.c
+++ b/terminal.c
@@ -34,6 +34,8 @@
#include "drv8320s.h"
#include "drv8323s.h"
#include "app.h"
+#include "comm_usb.h"
+#include "comm_usb_serial.h"
#include <string.h>
#include <stdio.h>
@@ -448,6 +450,13 @@ void terminal_process_string(char *str) {
commands_printf("FOC Current Offsets: %d %d %d",
curr0_offset, curr1_offset, curr2_offset);
+#ifdef COMM_USE_USB
+ commands_printf("USB config events: %d", comm_usb_serial_configured_cnt());
+ commands_printf("USB write timeouts: %u", comm_usb_get_write_timeout_cnt());
+#else
+ commands_printf("USB not enabled on hardware.");
+#endif
+
commands_printf(" ");
} else if (strcmp(argv[0], "foc_openloop") == 0) {
if (argc == 3) {
@@ -684,6 +693,8 @@ void terminal_process_string(char *str) {
encoder_sincos_get_signal_above_max_error_cnt(),
(double)encoder_sincos_get_signal_above_max_error_rate() * (double)100.0);
}
+ } else if (strcmp(argv[0], "uptime") == 0) {
+ commands_printf("Uptime: %.2f s\n", (double)chVTGetSystemTimeX() / (double)CH_CFG_ST_FREQUENCY);
}
// The help command
@@ -799,6 +810,9 @@ void terminal_process_string(char *str) {
commands_printf("encoder");
commands_printf(" Prints the status of the AS5047, AD2S1205, or TS5700N8501 encoder.");
+ commands_printf("uptime");
+ commands_printf(" Prints how many seconds have passed since boot.");
+
for (int i = 0;i < callback_write;i++) {
if (callbacks[i].cbf == 0) {
continue;
diff --git a/utils.c b/utils.c
index 4194f9605..646742461 100644
--- a/utils.c
+++ b/utils.c
@@ -344,7 +344,7 @@ float utils_fast_atan2(float y, float x) {
*/
bool utils_saturate_vector_2d(float *x, float *y, float max) {
bool retval = false;
- float mag = sqrtf(*x * *x + *y * *y);
+ float mag = sqrtf(SQ(*x) + SQ(*y));
max = fabsf(max);
if (mag < 1e-10) {
diff --git a/utils.h b/utils.h
index f6642437c..98713a73f 100644
--- a/utils.h
+++ b/utils.h
@@ -77,7 +77,7 @@ uint32_t utils_crc32c(uint8_t *data, uint32_t len);
* @param filter_constant
* Filter constant. Range 0.0 to 1.0, where 1.0 gives the unfiltered value.
*/
-#define UTILS_LP_FAST(value, sample, filter_constant) (value -= (filter_constant) * (value - (sample)))
+#define UTILS_LP_FAST(value, sample, filter_constant) (value -= (filter_constant) * ((value) - (sample)))
// Some constants
#define ONE_BY_SQRT3 (0.57735026919)