75/300 R2 support, terminal sync cmd, IMU support, option to disable permanent UART, collected timer functions in one place

Author: vedderb

CHANGELOG

@@ -1,3 +1,14 @@
+=== FW 3.52 ===
+* Added support for second revision of HW75/300 with separate UART for NRF51.
+* Added COMM_TERMINAL_CMD_SYNC, which does not drop commands when busy.
+* Added option to disable permanent UART.
+* Moved TIM5 to own file, and use it from other places.
+* Removed need for TIM2 in mcpwm.
+* Added utilization percentage to threads terminal command.
+* Added IMU interface.
+* Added support for the MPU9150 and MPU9250.
+* Added COMM_GET_IMU_DATA.
+
 === FW 3.51 ===
 * Fixed AS5047 error rate bug at position 0.
 * Increased threshold for AS5047 fault to 5 %.

Makefile

@@ -108,6 +108,7 @@ include hwconf/hwconf.mk
 include applications/applications.mk
 include nrf/nrf.mk
 include libcanard/canard.mk
+include imu/imu.mk
 
 # Define linker script file here
 LDSCRIPT= ld_eeprom_emu.ld
@@ -150,10 +151,13 @@ CSRC = $(STARTUPSRC) \
        mcpwm_foc.c \
        gpdrive.c \
        confgenerator.c \
+       timer.c \
+       i2c_bb.c \
        $(HWSRC) \
        $(APPSRC) \
        $(NRFSRC) \
-       $(CANARDSRC)
+       $(CANARDSRC) \
+       $(IMUSRC)
 
 # C++ sources that can be compiled in ARM or THUMB mode depending on the global
 # setting.
@@ -191,7 +195,8 @@ INCDIR = $(STARTUPINC) $(KERNINC) $(PORTINC) $(OSALINC) \
          $(HWINC) \
          $(APPINC) \
          $(NRFINC) \
-         $(CANARDINC)
+         $(CANARDINC) \
+         $(IMUINC)
 
 #
 # Project, sources and paths

appconf/appconf_default.h

@@ -48,6 +48,9 @@
 #ifndef APPCONF_PAIRING_DONE
 #define APPCONF_PAIRING_DONE				false
 #endif
+#ifndef APPCONF_PERMANENT_UART_ENABLED
+#define APPCONF_PERMANENT_UART_ENABLED		true
+#endif
 
 // The default app is UART in case the UART port is used for
 // firmware updates.

applications/app.c

@@ -114,7 +114,7 @@ void app_set_configuration(app_configuration *conf) {
 
 	app_ppm_configure(&appconf.app_ppm_conf);
 	app_adc_configure(&appconf.app_adc_conf);
-	app_uartcomm_configure(appconf.app_uart_baudrate);
+	app_uartcomm_configure(appconf.app_uart_baudrate, appconf.permanent_uart_enabled);
 	app_nunchuk_configure(&appconf.app_chuk_conf);
 
 #ifdef APP_CUSTOM_TO_USE

applications/app.h

@@ -45,7 +45,7 @@ float app_adc_get_voltage2(void);
 void app_uartcomm_start(void);
 void app_uartcomm_start_permanent(void);
 void app_uartcomm_stop(void);
-void app_uartcomm_configure(uint32_t baudrate);
+void app_uartcomm_configure(uint32_t baudrate, bool permanent_enabled);
 void app_uartcomm_send_packet(unsigned char *data, unsigned int len);
 
 void app_nunchuk_start(void);

applications/app_uartcomm.c

@@ -148,13 +148,29 @@ void app_uartcomm_send_packet(unsigned char *data, unsigned int len) {
 	chMtxUnlock(&send_mutex);
 }
 
-void app_uartcomm_configure(uint32_t baudrate) {
+void app_uartcomm_configure(uint32_t baudrate, bool permanent_enabled) {
 	uart_cfg.speed = baudrate;
 
 	if (thread_is_running) {
 		sdStart(&HW_UART_DEV, &uart_cfg);
 		uart_is_running = true;
 	}
+
+#ifdef HW_UART_P_DEV
+	if (permanent_enabled) {
+		palSetPadMode(HW_UART_P_TX_PORT, HW_UART_P_TX_PIN, PAL_MODE_ALTERNATE(HW_UART_P_GPIO_AF) |
+				PAL_STM32_OSPEED_HIGHEST |
+				PAL_STM32_PUDR_PULLUP);
+		palSetPadMode(HW_UART_P_RX_PORT, HW_UART_P_RX_PIN, PAL_MODE_ALTERNATE(HW_UART_P_GPIO_AF) |
+				PAL_STM32_OSPEED_HIGHEST |
+				PAL_STM32_PUDR_PULLUP);
+	} else {
+		palSetPadMode(HW_UART_P_TX_PORT, HW_UART_P_TX_PIN, PAL_MODE_INPUT);
+		palSetPadMode(HW_UART_P_RX_PORT, HW_UART_P_RX_PIN, PAL_MODE_INPUT);
+	}
+#else
+	(void)permanent_enabled;
+#endif
 }
 
 static THD_FUNCTION(packet_process_thread, arg) {

build_all/410_o_411_o_412/VESC_0005ohm.bin

@@ -217,6 +217,44 @@ cd $FWPATH
 make clean
 cd $DIR
 
+#################### HW 75_300_R2 ########################
+
+COPYDIR=75_300_R2
+rm -f $COPYDIR/*
+
+# default
+cd $FWPATH
+touch conf_general.h
+make -j8 build_args='-DHW_SOURCE=\"hw_75_300.c\" -DHW_HEADER=\"hw_75_300.h\" -DHW75_300_REV_2'
+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_75_300.c\" -DHW_HEADER=\"hw_75_300.h\" -DHW75_300_REV_2'
+cd $DIR
+cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_default_no_hw_limits.bin
+
+# ws2811
+cd $FWPATH
+touch conf_general.h
+make -j8 build_args='-DWS2811_ENABLE=1 -DHW_SOURCE=\"hw_75_300.c\" -DHW_HEADER=\"hw_75_300.h\" -DHW75_300_REV_2'
+cd $DIR
+cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_ws2811.bin
+
+# servoout
+cd $FWPATH
+touch conf_general.h
+make -j8 build_args='-DSERVO_OUT_ENABLE=1 -DHW_SOURCE=\"hw_75_300.c\" -DHW_HEADER=\"hw_75_300.h\" -DHW75_300_REV_2'
+cd $DIR
+cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_servoout.bin
+
+# Clean
+cd $FWPATH
+make clean
+cd $DIR
+
 #################### HW PALTA ########################
 
 COPYDIR=PALTA

build_all/410_o_411_o_412/VESC_default.bin

@@ -40,6 +40,7 @@
 #include "nrf_driver.h"
 #include "gpdrive.h"
 #include "confgenerator.h"
+#include "imu.h"
 
 #include <math.h>
 #include <string.h>
@@ -63,10 +64,12 @@ static void(* volatile appdata_func)(unsigned char *data, unsigned int len) = 0;
 static disp_pos_mode display_position_mode;
 static mutex_t print_mutex;
 static mutex_t send_buffer_mutex;
+static mutex_t terminal_mutex;
 
 void commands_init(void) {
 	chMtxObjectInit(&print_mutex);
 	chMtxObjectInit(&send_buffer_mutex);
+	chMtxObjectInit(&terminal_mutex);
 	chThdCreateStatic(blocking_thread_wa, sizeof(blocking_thread_wa), NORMALPRIO, blocking_thread, NULL);
 }
 
@@ -838,6 +841,86 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 		}
 	} break;
 
+	case COMM_TERMINAL_CMD_SYNC:
+		data[len] = '\0';
+		chMtxLock(&terminal_mutex);
+		terminal_process_string((char*)data);
+		chMtxUnlock(&terminal_mutex);
+		break;
+
+	case COMM_GET_IMU_DATA: {
+		int32_t ind = 0;
+		uint8_t send_buffer[70];
+		send_buffer[ind++] = packet_id;
+
+		int32_t ind2 = 0;
+		uint32_t mask = buffer_get_uint16(data, &ind2);
+
+		float rpy[3], acc[3], gyro[3], mag[3], q[4];
+		imu_get_rpy(rpy);
+		imu_get_accel(acc);
+		imu_get_gyro(gyro);
+		imu_get_mag(mag);
+		imu_get_quaternions(q);
+
+		buffer_append_uint16(send_buffer, mask, &ind);
+
+		if (mask & ((uint32_t)1 << 0)) {
+			buffer_append_float32_auto(send_buffer, rpy[0], &ind);
+		}
+		if (mask & ((uint32_t)1 << 1)) {
+			buffer_append_float32_auto(send_buffer, rpy[1], &ind);
+		}
+		if (mask & ((uint32_t)1 << 2)) {
+			buffer_append_float32_auto(send_buffer, rpy[2], &ind);
+		}
+
+		if (mask & ((uint32_t)1 << 3)) {
+			buffer_append_float32_auto(send_buffer, acc[0], &ind);
+		}
+		if (mask & ((uint32_t)1 << 4)) {
+			buffer_append_float32_auto(send_buffer, acc[1], &ind);
+		}
+		if (mask & ((uint32_t)1 << 5)) {
+			buffer_append_float32_auto(send_buffer, acc[2], &ind);
+		}
+
+		if (mask & ((uint32_t)1 << 6)) {
+			buffer_append_float32_auto(send_buffer, gyro[0], &ind);
+		}
+		if (mask & ((uint32_t)1 << 7)) {
+			buffer_append_float32_auto(send_buffer, gyro[1], &ind);
+		}
+		if (mask & ((uint32_t)1 << 8)) {
+			buffer_append_float32_auto(send_buffer, gyro[2], &ind);
+		}
+
+		if (mask & ((uint32_t)1 << 9)) {
+			buffer_append_float32_auto(send_buffer, mag[0], &ind);
+		}
+		if (mask & ((uint32_t)1 << 10)) {
+			buffer_append_float32_auto(send_buffer, mag[1], &ind);
+		}
+		if (mask & ((uint32_t)1 << 11)) {
+			buffer_append_float32_auto(send_buffer, mag[2], &ind);
+		}
+
+		if (mask & ((uint32_t)1 << 12)) {
+			buffer_append_float32_auto(send_buffer, q[0], &ind);
+		}
+		if (mask & ((uint32_t)1 << 13)) {
+			buffer_append_float32_auto(send_buffer, q[1], &ind);
+		}
+		if (mask & ((uint32_t)1 << 14)) {
+			buffer_append_float32_auto(send_buffer, q[2], &ind);
+		}
+		if (mask & ((uint32_t)1 << 15)) {
+			buffer_append_float32_auto(send_buffer, q[3], &ind);
+		}
+
+		reply_func(send_buffer, ind);
+	} break;
+
 	// Blocking commands. Only one of them runs at any given time, in their
 	// own thread. If other blocking commands come before the previous one has
 	// finished, they are discarded.
@@ -1222,7 +1305,9 @@ static THD_FUNCTION(blocking_thread, arg) {
 
 		case COMM_TERMINAL_CMD:
 			data[len] = '\0';
+			chMtxLock(&terminal_mutex);
 			terminal_process_string((char*)data);
+			chMtxUnlock(&terminal_mutex);
 			break;
 
 		case COMM_PING_CAN: {

build_all/410_o_411_o_412/VESC_default_no_hw_limits.bin

@@ -22,7 +22,7 @@
 
 // Firmware version
 #define FW_VERSION_MAJOR		3
-#define FW_VERSION_MINOR		51
+#define FW_VERSION_MINOR		52
 
 #include "datatypes.h"
 
@@ -41,12 +41,6 @@
 // Disable hardware limits on configuration parameters
 //#define DISABLE_HW_LIMITS
 
-// Benjamins first HW60 PCB with PB5 and PB6 swapped
-//#define HW60_VEDDER_FIRST_PCB
-
-// Benjamins first HW75_300 PCB with different LED pins and motor temp error
-//#define HW75_300_VEDDER_FIRST_PCB
-
 /*
  * Select only one hardware version, if it is not passed
  * as an argument.
@@ -70,6 +64,9 @@
 //#define HW_SOURCE "hw_410.c" // Also for 4.11 and 4.12
 //#define HW_HEADER "hw_410.h" // Also for 4.11 and 4.12
 
+// Benjamins first HW60 PCB with PB5 and PB6 swapped
+//#define HW60_VEDDER_FIRST_PCB
+
 #define HW_SOURCE "hw_60.c"
 #define HW_HEADER "hw_60.h"
 
@@ -91,6 +88,12 @@
 //#define HW_SOURCE "hw_tp.c"
 //#define HW_HEADER "hw_tp.h"
 
+// Benjamins first HW75_300 PCB with different LED pins and motor temp error
+//#define HW75_300_VEDDER_FIRST_PCB
+
+// Second revision with separate UART for NRF51
+//#define HW75_300_REV_2
+
 //#define HW_SOURCE "hw_75_300.c"
 //#define HW_HEADER "hw_75_300.h"
 

build_all/410_o_411_o_412/VESC_servoout.bin

@@ -147,6 +147,7 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
 	buffer_append_uint16(buffer, conf->send_can_status_rate_hz, &ind);
 	buffer[ind++] = conf->can_baud_rate;
 	buffer[ind++] = conf->pairing_done;
+	buffer[ind++] = conf->permanent_uart_enabled;
 	buffer[ind++] = conf->uavcan_enable;
 	buffer[ind++] = (uint8_t)conf->uavcan_esc_index;
 	buffer[ind++] = conf->app_to_use;
@@ -363,6 +364,7 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
 	conf->send_can_status_rate_hz = buffer_get_uint16(buffer, &ind);
 	conf->can_baud_rate = buffer[ind++];
 	conf->pairing_done = buffer[ind++];
+	conf->permanent_uart_enabled = buffer[ind++];
 	conf->uavcan_enable = buffer[ind++];
 	conf->uavcan_esc_index = buffer[ind++];
 	conf->app_to_use = buffer[ind++];
@@ -563,6 +565,7 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
 	conf->send_can_status_rate_hz = APPCONF_SEND_CAN_STATUS_RATE_HZ;
 	conf->can_baud_rate = APPCONF_CAN_BAUD_RATE;
 	conf->pairing_done = APPCONF_PAIRING_DONE;
+	conf->permanent_uart_enabled = APPCONF_PERMANENT_UART_ENABLED;
 	conf->uavcan_enable = APPCONF_UAVCAN_ENABLE;
 	conf->uavcan_esc_index = APPCONF_UAVCAN_ESC_INDEX;
 	conf->app_to_use = APPCONF_APP_TO_USE;

build_all/410_o_411_o_412/VESC_ws2811.bin

@@ -9,7 +9,7 @@
 
 // Constants
 #define MCCONF_SIGNATURE		1753222668
-#define APPCONF_SIGNATURE		3794395266
+#define APPCONF_SIGNATURE		1146628643
 
 // Functions
 int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);

build_all/46_o_47/VESC_0005ohm.bin

@@ -129,7 +129,7 @@ typedef struct {
 	float comm_time_sum;
 	float comm_time_sum_min_rpm;
 	int32_t comms;
-	uint32_t time_at_comm;
+	float time_at_comm;
 } mc_rpm_dep_struct;
 
 typedef enum {
@@ -482,6 +482,7 @@ typedef struct {
 	uint32_t send_can_status_rate_hz;
 	CAN_BAUD can_baud_rate;
 	bool pairing_done;
+	bool permanent_uart_enabled;
 
 	// UAVCAN
 	bool uavcan_enable;
@@ -571,7 +572,9 @@ typedef enum {
 	COMM_ERASE_NEW_APP_ALL_CAN,
 	COMM_WRITE_NEW_APP_DATA_ALL_CAN,
 	COMM_PING_CAN,
-	COMM_APP_DISABLE_OUTPUT
+	COMM_APP_DISABLE_OUTPUT,
+	COMM_TERMINAL_CMD_SYNC,
+	COMM_GET_IMU_DATA
 } COMM_PACKET_ID;
 
 // CAN commands
@@ -729,4 +732,17 @@ typedef enum {
 	NRF_PAIR_FAIL
 } NRF_PAIR_RES;
 
+// Orientation data
+typedef struct {
+	float q0;
+	float q1;
+	float q2;
+	float q3;
+	float integralFBx;
+	float integralFBy;
+	float integralFBz;
+	float accMagP;
+	int initialUpdateDone;
+} ATTITUDE_INFO;
+
 #endif /* DATATYPES_H_ */